diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 07e7e6b5ad169..6acb74342c90f 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -23,6 +23,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp src/simple_planning_simulator/utils/csv_loader.cpp + src/simple_planning_simulator/utils/mechanical_controller.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learning_based_vehicle_model_INCLUDE_DIRS}) # Node executable diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 1d91d5e6149b4..cd362c0115358 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -65,32 +65,38 @@ The purpose of this simulator is for the integration test of planning and contro - `DELAY_STEER_ACC_GEARED_WO_FALL_GUARD` - `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle. - `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../learning_based_vehicle_model). -- `ACTUATION_CMD`: A simulator model that receives `ACTUATION_CMD`. In this case, the `raw_vehicle_cmd_converter` is also launched. + +The following models receive `ACTUATION_CMD` generated from `raw_vehicle_cmd_converter`. Therefore, when these models are selected, the `raw_vehicle_cmd_converter` is also launched. + +- `ACTUATION_CMD`: This model only converts accel/brake commands and use steer command as it is. +- `ACTUATION_CMD_STEER_MAP`: The steer command is converted to the steer rate command using the steer map. +- `ACTUATION_CMD_VGR`: The steer command is converted to the steer rate command using the variable gear ratio. +- `ACTUATION_CMD_MECHANICAL`: This model has mechanical dynamics and controller for that. The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. 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 | D_ST_A_G_WO_FG | D_ST_M_ACC_G | L_S_V | Default value | unit | -| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------- | :----------- | :---- | :------------ | :------ | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | o | x | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | o | x | 0.24 | [s] | -| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | x | x | 0.25 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | o | x | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | o | x | 0.27 | [s] | -| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | o | x | x | 0.0 | [rad] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | x | x | 0.5 | [s] | -| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | o | x | 50.0 | [m/s] | -| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | o | x | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | o | x | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | o | x | 5.0 | [rad/s] | -| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | o | x | 0.0 | [rad] | -| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | o | x | x | 1.0 | [-] | -| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | o | x | x | 1.0 | [-] | -| acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | x | o | x | - | [-] | -| model_module_paths | string | path to a python module where the model is implemented | x | x | x | x | x | x | x | x | o | - | [-] | -| model_param_paths | string | path to the file where model parameters are stored (can be empty string if no parameter file is required) | x | x | x | x | x | x | x | x | o | - | [-] | -| model_class_names | string | name of the class that implements the model | x | x | x | x | x | x | x | x | o | - | [-] | +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_A_G_WO_FG | D_ST_M_ACC_G | L_S_V | A_C | Default value | unit | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------- | :----------- | :---- | :-- | :------------ | :------ | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | o | x | x | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | o | x | o | 0.24 | [s] | +| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | x | x | x | 0.25 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | o | x | x | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | o | x | o | 0.27 | [s] | +| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | o | x | x | o | 0.0 | [rad] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | x | x | x | 0.5 | [s] | +| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | o | x | x | 50.0 | [m/s] | +| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | o | x | x | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | o | x | o | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | o | x | o | 5.0 | [rad/s] | +| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | o | x | o | 0.0 | [rad] | +| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | o | x | x | x | 1.0 | [-] | +| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | o | x | x | x | 1.0 | [-] | +| acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | x | o | x | x | - | [-] | +| model_module_paths | string | path to a python module where the model is implemented | x | x | x | x | x | x | x | x | o | x | - | [-] | +| model_param_paths | string | path to the file where model parameters are stored (can be empty string if no parameter file is required) | x | x | x | x | x | x | x | x | o | x | - | [-] | +| model_class_names | string | name of the class that implements the model | x | x | x | x | x | x | x | x | o | x | - | [-] | _Note:_ Parameters `model_module_paths`, `model_param_paths`, and `model_class_names` need to have the same length. @@ -125,32 +131,8 @@ default, 0.00, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 1 ##### ACTUATION_CMD model -The simple_planning_simulator usually operates by receiving Control commands, but when the `ACTUATION_CMD` model is selected, it receives Actuation commands instead of Control commands. This model can simulate the motion using the vehicle command that is actually sent to the real vehicle. Therefore, when this model is selected, the `raw_vehicle_cmd_converter` is also launched. - -`convert_steer_cmd_method` has two options: "vgr" and "steer_map". If you choose "vgr" (variable gear ratio), it is assumed that the steering wheel angle is sent as the actuation command, and the value is converted to the steering tire angle to move the model. If you choose "steer_map", it is assumed that an arbitrary value is sent as the actuation command, and the value is converted to the steering tire rate to move the model. An arbitrary value is like EPS (Electric Power Steering) Voltage . `enable_pub_steer` determines whether to publish the steering tire angle. If false, it is expected to be converted and published from actuation_status in other nodes (e.g. raw_vehicle_cmd_converter). - -![vgr_sim](./media/vgr_sim.drawio.svg) - -```yaml - -``` - -The parameters used in the ACTUATION_CMD are as follows. - -| Name | Type | Description | unit | -| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--- | -| accel_time_delay | double | dead time for the acceleration input | [s] | -| accel_time_constant | double | time constant of the 1st-order acceleration dynamics | [s] | -| brake_time_delay | double | dead time for the brake input | [s] | -| brake_time_constant | double | time constant of the 1st-order brake dynamics | [s] | -| convert_accel_cmd | bool | If true, it is assumed that the command is received converted to an accel actuation value, and it is converted back to acceleration value inside the simulator. | [-] | -| convert_brake_cmd | bool | If true, it is assumed that the command is received converted to a brake actuation value, and it is converted back to acceleration value inside the simulator. | [-] | -| convert_steer_cmd | bool | If true, it is assumed that the command is received converted to a steer actuation value, and it is converted back to steer rate value inside the simulator. | [-] | -| convert_steer_cmd_method | bool | method to convert steer command. You can choose from "vgr" and "steer_map". | [-] | -| vgr_coef_a | double | the value of the coefficient a of the variable gear ratio | [-] | -| vgr_coef_b | double | the value of the coefficient b of the variable gear ratio | [-] | -| vgr_coef_c | double | the value of the coefficient c of the variable gear ratio | [-] | -| enable_pub_steer | bool | whether to publish the steering tire angle. if false, it is expected to be converted and published from actuation_status in other nodes (e.g. raw_vehicle_cmd_converter) | [-] | +The simple_planning_simulator usually operates by receiving Control commands, but when the `ACTUATION_CMD*` model is selected, it receives Actuation commands instead of Control commands. This model can simulate the motion using the vehicle command that is actually sent to the real vehicle. Therefore, when this model is selected, the `raw_vehicle_cmd_converter` is also launched. +Please refer to the [actuation_cmd_sim.md](./docs/actuation_cmd_sim.md) for more details. diff --git a/simulator/simple_planning_simulator/docs/actuation_cmd_sim.md b/simulator/simple_planning_simulator/docs/actuation_cmd_sim.md new file mode 100644 index 0000000000000..5219b20b6dbaf --- /dev/null +++ b/simulator/simple_planning_simulator/docs/actuation_cmd_sim.md @@ -0,0 +1,105 @@ +# ACTUATION_CMD model + +The simple_planning_simulator usually operates by receiving Control commands, but when the `ACTUATION_CMD*` model is selected, it receives Actuation commands instead of Control commands. This model can simulate the motion using the vehicle command that is actually sent to the real vehicle. Therefore, when this model is selected, the `raw_vehicle_cmd_converter` is also launched. + +## ACTUATION_CMD + +This model receives the accel/brake commands and converts them using the map to calculate the motion of the model. The steer command is used as it is. +Please make sure that the raw_vehicle_cmd_converter is configured as follows. + +```yaml +convert_accel_cmd: true +convert_brake_cmd: true +convert_steer_cmd: false +``` + +## ACTUATION_CMD_STEER_MAP + +This model is inherited from ACTUATION_CMD and receives steering arbitrary value as the actuation command. +The value is converted to the steering tire rate to calculate the motion of the model. An arbitrary value is like EPS (Electric Power Steering) Voltage. + +Please make sure that the raw_vehicle_cmd_converter is configured as follows. + +```yaml +convert_accel_cmd: true +convert_brake_cmd: true +convert_steer_cmd: true +``` + +## ACTUATION_CMD_VGR + +This model is inherited from ACTUATION_CMD and steering wheel angle is sent as the actuation command. +The value is converted to the steering tire angle to calculate the motion of the model. + +Please make sure that the raw_vehicle_cmd_converter is configured as follows. + +```yaml +convert_accel_cmd: true +convert_brake_cmd: true +convert_steer_cmd: true +``` + +![vgr_sim](../media/vgr_sim.drawio.svg) + +## ACTUATION_CMD_MECHANICAL + +This model is inherited from ACTUATION_CMD_VGR nad has mechanical dynamics and controller for that to simulate the mechanical structure and software of the real vehicle. + +Please make sure that the raw_vehicle_cmd_converter is configured as follows. + +```yaml +convert_accel_cmd: true +convert_brake_cmd: true +convert_steer_cmd: true +``` + +The mechanical structure of the vehicle is as follows. + +![mechanical_controller](../media/mechanical_controller.drawio.svg) + +The vehicle side software assumes that it has limiters, PID controllers, power steering, etc. for the input. +The conversion in the power steering is approximated by a polynomial. +Steering Dynamics is a model that represents the motion of the tire angle when the Steering Torque is input. It is represented by the following formula. + +```math +\begin{align} +\dot{\theta} &= \omega \\ +\dot{\omega} &= \frac{1}{I} (T_{\text{input}} - D \omega - K \theta - \text{sign}(\omega) F_{\text{friction}} ) \\ +\end{align} +``` + +In this case, + +- $\theta$ : Tire angle +- $\omega$ : Tire angular velocity +- $T_{\text{input}}$ : Input torque +- $D$ : Damping coefficient +- $K$ : Spring constant +- $F_{\text{friction}}$ : Friction force +- $I$ : Moment of inertia + +Also, this dynamics has a dead zone. +The steering rotation direction is different from the steering torque input direction, and the steering torque input is less than the dead zone threshold, it enters the dead zone. Once it enters the dead zone, it is judged to be in the dead zone until there is a steering input above the dead zone threshold. When in the dead zone, the steering tire angle does not move. + +Please refer to the following file for the values of the parameters that have been system-identified using the actual vehicle's driving data. +The blue line is the control input, the green line is the actual vehicle's tire angle output, and the red line is the simulator's tire angle output. +[mechanical_sample_param](../param/simple_planning_simulator_mechanical_sample.param.yaml) + +This model has a smaller sum of errors with the observed values of the actual vehicle than when tuned with a normal first-order lag model. For details, please refer to [#9252](https://github.com/autowarefoundation/autoware.universe/pull/9300). + +![mechanical_controller_system_identification](../media/mechanical_controller_system_identification.png) + +The parameters used in the ACTUATION_CMD are as follows. + +| Name | Type | Description | unit | +| :------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--- | +| accel_time_delay | double | dead time for the acceleration input | [s] | +| accel_time_constant | double | time constant of the 1st-order acceleration dynamics | [s] | +| brake_time_delay | double | dead time for the brake input | [s] | +| brake_time_constant | double | time constant of the 1st-order brake dynamics | [s] | +| convert_accel_cmd | bool | If true, it is assumed that the command is received converted to an accel actuation value, and it is converted back to acceleration value inside the simulator. | [-] | +| convert_brake_cmd | bool | If true, it is assumed that the command is received converted to a brake actuation value, and it is converted back to acceleration value inside the simulator. | [-] | +| vgr_coef_a | double | the value of the coefficient a of the variable gear ratio | [-] | +| vgr_coef_b | double | the value of the coefficient b of the variable gear ratio | [-] | +| vgr_coef_c | double | the value of the coefficient c of the variable gear ratio | [-] | +| enable_pub_steer | bool | whether to publish the steering tire angle. if false, it is expected to be converted and published from actuation_status in other nodes (e.g. raw_vehicle_cmd_converter) | [-] | 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 76fdf5bdda6c9..3e54bca245f82 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 @@ -224,7 +224,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node DELAY_STEER_MAP_ACC_GEARED = 6, LEARNED_STEER_VEL = 7, DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8, - ACTUATION_CMD = 9 + ACTUATION_CMD = 9, + ACTUATION_CMD_VGR = 10, + ACTUATION_CMD_MECHANICAL = 11, + ACTUATION_CMD_STEER_MAP = 12, } 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/utils/mechanical_controller.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/mechanical_controller.hpp new file mode 100644 index 0000000000000..fcb8d41dd72b5 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/mechanical_controller.hpp @@ -0,0 +1,208 @@ +// 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__UTILS__MECHANICAL_CONTROLLER_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include + +namespace autoware::simple_planning_simulator::utils::mechanical_controller +{ + +using DelayBuffer = std::deque>; +using DelayOutput = std::pair, DelayBuffer>; + +DelayOutput delay( + const double signal, const double delay_time, const DelayBuffer & buffer, + const double elapsed_time); + +double sign(const double x); + +double apply_limits( + const double current_angle, const double previous_angle, const double angle_limit, + const double rate_limit, const double dt); + +double feedforward(const double input_angle, const double ff_gain); + +double polynomial_transform( + const double torque, const double speed, const double a, const double b, const double c, + const double d, const double e, const double f, const double g, const double h); + +struct PIDControllerParams +{ + double kp{0.0}; + double ki{0.0}; + double kd{0.0}; +}; + +struct PIDControllerState +{ + double integral{0.0}; + double error{0.0}; +}; + +class PIDController +{ +public: + PIDController(const double kp, const double ki, const double kd); + + [[nodiscard]] double compute( + const double error, const double integral, const double prev_error, const double dt) const; + + void update_state(const double error, const double dt); + + void update_state( + const double k1_error, const double k2_error, const double k3_error, const double k4_error, + const double dt); + + [[nodiscard]] PIDControllerState get_state() const; + + void clear_state(); + +private: + double kp_{0.0}, ki_{0.0}, kd_{0.0}; + PIDControllerState state_; +}; + +struct SteeringDynamicsParams +{ + double angular_position{0.0}; + double angular_velocity{0.0}; + double inertia{0.0}; + double damping{0.0}; + double stiffness{0.0}; + double friction{0.0}; + double dead_zone_threshold{0.0}; +}; + +struct SteeringDynamicsState +{ + double angular_position{0.0}; + double angular_velocity{0.0}; + bool is_in_dead_zone{false}; +}; + +struct SteeringDynamicsDeltaState +{ + double d_angular_position{0.0}; + double d_angular_velocity{0.0}; +}; + +/** + * @brief SteeringDynamics class + * @details This class calculates the dynamics which receives the steering torque and outputs the + * steering tire angle. The steering system is modeled as a spring-damper system with friction and + * dead zone. + */ +class SteeringDynamics +{ +public: + SteeringDynamics( + const double angular_position, const double angular_velocity, const double inertia, + const double damping, const double stiffness, const double friction, + const double dead_zone_threshold); + + [[nodiscard]] bool is_in_dead_zone( + const SteeringDynamicsState & state, const double input_torque) const; + + [[nodiscard]] SteeringDynamicsDeltaState calc_model( + const SteeringDynamicsState & state, const double input_torque) const; + + void set_state(const SteeringDynamicsState & state); + + [[nodiscard]] SteeringDynamicsState get_state() const; + + void clear_state(); + + void set_steer(const double steer); + +private: + SteeringDynamicsState state_; + const double inertia_; + const double damping_; + const double stiffness_; + const double friction_; + const double dead_zone_threshold_; +}; + +struct StepResult +{ + DelayBuffer delay_buffer{}; + double pid_error{0.0}; + SteeringDynamicsDeltaState dynamics_d_state{}; + bool is_in_dead_zone{false}; +}; + +struct MechanicalParams +{ + double kp{0.0}; + double ki{0.0}; + double kd{0.0}; + double ff_gain{0.0}; + double dead_zone_threshold{0.0}; + double poly_a{0.0}; + double poly_b{0.0}; + double poly_c{0.0}; + double poly_d{0.0}; + double poly_e{0.0}; + double poly_f{0.0}; + double poly_g{0.0}; + double poly_h{0.0}; + double inertia{0.0}; + double damping{0.0}; + double stiffness{0.0}; + double friction{0.0}; + double delay_time{0.0}; + + // limit + double angle_limit{0.0}; + double rate_limit{0.0}; + double steering_torque_limit{0.0}; + double torque_delay_time{0.0}; +}; + +class MechanicalController +{ +public: + explicit MechanicalController(const MechanicalParams & mechanical_params); + + [[maybe_unused]] double update_euler( + const double input_angle, const double speed, const double prev_input_angle, const double dt); + + double update_runge_kutta( + const double input_angle, const double speed, const double prev_input_angle, const double dt); + + void clear_state(); + + void set_steer(const double steer); + +private: + DelayBuffer delay_buffer_; + PIDController pid_; + SteeringDynamics steering_dynamics_; + const MechanicalParams params_; + + [[nodiscard]] StepResult run_one_step( + const double input_angle, const double speed, const double prev_input_angle, const double dt, + const DelayBuffer & delay_buffer, const PIDController & pid, + const SteeringDynamics & dynamics) const; +}; + +} // namespace autoware::simple_planning_simulator::utils::mechanical_controller + +#endif // SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp index b78ec8ccd538a..25fac72d283ee 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp @@ -19,15 +19,20 @@ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" #include "simple_planning_simulator/utils/csv_loader.hpp" +#include "simple_planning_simulator/utils/mechanical_controller.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include +#include #include #include #include #include +using autoware::simple_planning_simulator::utils::mechanical_controller::MechanicalController; +using autoware::simple_planning_simulator::utils::mechanical_controller::MechanicalParams; + /** * @class ActuationMap * @brief class to convert from actuation command to control command @@ -101,10 +106,8 @@ class BrakeMap : public ActuationMap class SimModelActuationCmd : public SimModelInterface { public: - enum class ActuationSimType { VGR, STEER_MAP }; - /** - * @brief constructor (adaptive gear ratio conversion model) + * @brief constructor (only longitudinal) * @param [in] vx_lim velocity limit [m/s] * @param [in] steer_lim steering limit [rad] * @param [in] vx_rate_lim acceleration limit [m/ss] @@ -120,49 +123,15 @@ class SimModelActuationCmd : public SimModelInterface * @param [in] steer_bias steering bias [rad] * @param [in] convert_accel_cmd flag to convert accel command * @param [in] convert_brake_cmd flag to convert brake command - * @param [in] convert_steer_cmd flag to convert steer command - * @param [in] accel_map_path path to csv file for accel conversion map - * @param [in] brake_map_path path to csv file for brake conversion map - * @param [in] vgr_coef_a coefficient for variable gear ratio - * @param [in] vgr_coef_b coefficient for variable gear ratio - * @param [in] vgr_coef_c coefficient for variable gear ratio - */ - SimModelActuationCmd( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double accel_delay, double accel_time_constant, double brake_delay, - double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, - bool convert_accel_cmd, bool convert_brake_cmd, bool convert_steer_cmd, - std::string accel_map_path, std::string brake_map_path, double vgr_coef_a, double vgr_coef_b, - double vgr_coef_c); - - /** - * @brief constructor (steer map conversion model) - * @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] accel_delay time delay for accel command [s] - * @param [in] acc_time_constant time constant for 1D model of accel dynamics - * @param [in] brake_delay time delay for brake command [s] - * @param [in] brake_time_constant time constant for 1D model of brake 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_bias steering bias [rad] - * @param [in] convert_accel_cmd flag to convert accel command - * @param [in] convert_brake_cmd flag to convert brake command - * @param [in] convert_steer_cmd flag to convert steer command * @param [in] accel_map_path path to csv file for accel conversion map * @param [in] brake_map_path path to csv file for brake conversion map - * @param [in] steer_map_path path to csv file for steer conversion map */ SimModelActuationCmd( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double accel_delay, double accel_time_constant, double brake_delay, double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, - bool convert_accel_cmd, bool convert_brake_cmd, bool convert_steer_cmd, - std::string accel_map_path, std::string brake_map_path, std::string steer_map_path); + bool convert_accel_cmd, bool convert_brake_cmd, std::string accel_map_path, + std::string brake_map_path); /** * @brief default destructor @@ -179,7 +148,7 @@ class SimModelActuationCmd : public SimModelInterface */ bool shouldPublishActuationStatus() const override { return true; } -private: +protected: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant enum IDX { @@ -198,39 +167,29 @@ class SimModelActuationCmd : public SimModelInterface GEAR, }; - 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 wheelbase_; //!< @brief vehicle wheelbase length [m] - - std::deque accel_input_queue_; //!< @brief buffer for accel command - std::deque brake_input_queue_; //!< @brief buffer for brake command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double accel_delay_; //!< @brief time delay for accel command [s] - const double accel_time_constant_; //!< @brief time constant for accel dynamics - const double brake_delay_; //!< @brief time delay for brake command [s] - const double brake_time_constant_; //!< @brief time constant for brake 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_bias_; //!< @brief steering angle bias [rad] - - bool convert_accel_cmd_; - bool convert_brake_cmd_; - bool convert_steer_cmd_; - - AccelMap accel_map_; - BrakeMap brake_map_; - ActuationMap steer_map_; - - // steer map conversion model - - // adaptive gear ratio conversion model - double vgr_coef_a_; - double vgr_coef_b_; - double vgr_coef_c_; - - ActuationSimType actuation_sim_type_{ActuationSimType::VGR}; + const double vx_lim_{0.0}; //!< @brief velocity limit [m/s] + const double vx_rate_lim_{0.0}; //!< @brief acceleration limit [m/ss] + const double steer_lim_{0.0}; //!< @brief steering limit [rad] + const double steer_rate_lim_{0.0}; //!< @brief steering angular velocity limit [rad/s] + const double wheelbase_{0.0}; //!< @brief vehicle wheelbase length [m] + + std::deque accel_input_queue_{}; //!< @brief buffer for accel command + std::deque brake_input_queue_{}; //!< @brief buffer for brake command + std::deque steer_input_queue_{}; //!< @brief buffer for steering command + const double accel_delay_{0.0}; //!< @brief time delay for accel command [s] + const double accel_time_constant_{0.0}; //!< @brief time constant for accel dynamics + const double brake_delay_{0.0}; //!< @brief time delay for brake command [s] + const double brake_time_constant_{0.0}; //!< @brief time constant for brake dynamics + const double steer_delay_{0.0}; //!< @brief time delay for steering command [s] + const double steer_time_constant_{0.0}; //!< @brief time constant for steering dynamics + const double steer_bias_{0.0}; //!< @brief steering angle bias [rad] + + bool convert_accel_cmd_{false}; + bool convert_brake_cmd_{false}; + bool convert_steer_cmd_{false}; + + AccelMap accel_map_{}; + BrakeMap brake_map_{}; /** * @brief set queue buffer for input command @@ -284,6 +243,24 @@ class SimModelActuationCmd : public SimModelInterface */ void update(const double & dt) override; + /** + * @brief calculate derivative of longitudinal states + * @param [in] state current model state + * @param [in] input input vector to model + * @return derivative of longitudinal states except steering + */ + Eigen::VectorXd calcLongitudinalModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input); + + /** + * @brief calculate derivative of lateral states + * @param [in] steer current steering angle [rad] + * @param [in] steer_input desired steering angle [rad] + * @param [in] vel current velocity [m/s] + * @return derivative of lateral states + */ + virtual double calcLateralModel(const double steer, const double steer_des, const double vel); + /** * @brief calculate derivative of states with time delay steering model * @param [in] state current model state @@ -301,7 +278,35 @@ class SimModelActuationCmd : public SimModelInterface void updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt); +}; +class SimModelActuationCmdSteerMap : public SimModelActuationCmd +{ +public: + SimModelActuationCmdSteerMap( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, std::string accel_map_path, + std::string brake_map_path, std::string steer_map_path); + +private: + double calcLateralModel(const double steer, const double steer_des, const double vel) override; + + ActuationMap steer_map_; +}; + +class SimModelActuationCmdVGR : public SimModelActuationCmd +{ +public: + SimModelActuationCmdVGR( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, std::string accel_map_path, + std::string brake_map_path, double vgr_coef_a, double vgr_coef_b, double vgr_coef_c); + +protected: /** * @brief calculate steering tire command * @param [in] vel current velocity [m/s] @@ -321,6 +326,75 @@ class SimModelActuationCmd : public SimModelInterface * @return variable gear ratio */ double calculateVariableGearRatio(const double vel, const double steer_wheel) const; + +private: + double calcLateralModel(const double steer, const double steer_des, const double vel) override; + + // adaptive gear ratio conversion model + double vgr_coef_a_; + double vgr_coef_b_; + double vgr_coef_c_; +}; + +class SimModelActuationCmdMechanical : public SimModelActuationCmdVGR +{ +public: + /** + * @brief constructor (mechanical model) + * @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] accel_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] brake_delay time delay for brake command [s] + * @param [in] brake_time_constant time constant for 1D model of brake 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_bias steering bias [rad] + * @param [in] convert_accel_cmd flag to convert accel command + * @param [in] convert_brake_cmd flag to convert brake command + * @param [in] convert_steer_cmd flag to convert steer command + * @param [in] accel_map_path path to csv file for accel conversion map + * @param [in] brake_map_path path to csv file for brake conversion map + * @param [in] vgr_coef_a coefficient for variable gear ratio + * @param [in] vgr_coef_b coefficient for variable gear ratio + * @param [in] vgr_coef_c coefficient for variable gear ratio + */ + SimModelActuationCmdMechanical( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, std::string accel_map_path, + std::string brake_map_path, double vgr_coef_a, double vgr_coef_b, double vgr_coef_c, + MechanicalParams mechanical_params); + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief update vehicle states with controller + * @details In updateRungeKutta, calcModel is called four times, but the internal state of PID and + * Dynamics should not be updated. Therefore, a method is prepared to update the internal state + * only once at the end without using the updateRungeKutta of the interface. + */ + void updateRungeKuttaWithController(const double dt, const Eigen::VectorXd & input); + + /** + * @brief set state + * @details This model needs to update mechanical dynamics state too + * @param [in] state state vector + */ + void setState(const Eigen::VectorXd & state) override; + +private: + std::unique_ptr mechanical_controller_; + double prev_steer_tire_des_{0.0}; // }; #endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index f8d682a6e851c..5ca886b1b5847 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -64,12 +64,6 @@ class SimModelInterface */ void getInput(Eigen::VectorXd & input); - /** - * @brief set state vector of model - * @param [in] state state vector - */ - void setState(const Eigen::VectorXd & state); - /** * @brief set input vector of model * @param [in] input input vector @@ -96,6 +90,14 @@ class SimModelInterface */ void updateEuler(const double & dt, const Eigen::VectorXd & input); + /** + * @brief set state vector of model + * @details In some sim models, the state member should be updated as well. Therefore, this + * function is defined as virtual. + * @param [in] state state vector + */ + virtual void setState(const Eigen::VectorXd & state); + /** * @brief update vehicle states * @param [in] dt delta time [s] diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 9b31926a53d57..aed088de9fd22 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -106,10 +106,9 @@ def launch_setup(context, *args, **kwargs): # Determine if we should launch raw_vehicle_cmd_converter based on the vehicle_model_type with open(simulator_model_param_path, "r") as f: simulator_model_param_yaml = yaml.safe_load(f) - launch_vehicle_cmd_converter = ( - simulator_model_param_yaml["/**"]["ros__parameters"].get("vehicle_model_type") - == "ACTUATION_CMD" - ) + launch_vehicle_cmd_converter = "ACTUATION_CMD" in simulator_model_param_yaml["/**"][ + "ros__parameters" + ].get("vehicle_model_type") # 1) Launch only simple_planning_simulator_node if not launch_vehicle_cmd_converter: diff --git a/simulator/simple_planning_simulator/media/mechanical_controller.drawio.svg b/simulator/simple_planning_simulator/media/mechanical_controller.drawio.svg new file mode 100644 index 0000000000000..a595b2479d4c6 --- /dev/null +++ b/simulator/simple_planning_simulator/media/mechanical_controller.drawio.svg @@ -0,0 +1,602 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Desired Steering Whell Angle +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ PID +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ FF +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+
+ + Power steering +
+ +
+ (polynominal) +
+
+
+ + + + + + + + + + + + + + +
+
+
Driver Torque
+
+
+ + + + + + + + + + + + + + +
+
+
+ Steering Torque +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Delay +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Steering Dynamics +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ VGR +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ VGR +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Limiter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Observed Steering Tire Angle +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Observed Steering +
Wheel Angle
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Mechanical Controller +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Simulator Model +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Desired Steering +
Tire Angle
+
+
+
+ + + + + + + + + + diff --git a/simulator/simple_planning_simulator/media/mechanical_controller_system_identification.png b/simulator/simple_planning_simulator/media/mechanical_controller_system_identification.png new file mode 100644 index 0000000000000..f96a5848b7fbc Binary files /dev/null and b/simulator/simple_planning_simulator/media/mechanical_controller_system_identification.png differ diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_mechanical_sample.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_mechanical_sample.param.yaml new file mode 100644 index 0000000000000..e844b07ba0693 --- /dev/null +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_mechanical_sample.param.yaml @@ -0,0 +1,62 @@ +/**: + ros__parameters: + simulated_frame_id: "base_link" + origin_frame_id: "map" + vehicle_model_type: "ACTUATION_CMD_MECHANICAL" + initialize_source: "INITIAL_POSE_TOPIC" + timer_sampling_time_ms: 25 + add_measurement_noise: False + enable_road_slope_simulation: True + vel_lim: 50.0 + vel_rate_lim: 7.0 + steer_lim: 1.0 + steer_rate_lim: 5.0 + acc_time_delay: 0.1 + acc_time_constant: 0.1 + steer_time_delay: 0.0 + steer_time_constant: 0.001 + steer_dead_band: 0.0 + x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate + y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate + + accel_time_delay: 0.1 + accel_time_constant: 0.1 + brake_time_delay: 0.1 + brake_time_constant: 0.1 + accel_map_path: $(find-pkg-share simple_planning_simulator)/data/actuation_cmd_map/accel_map.csv + brake_map_path: $(find-pkg-share simple_planning_simulator)/data/actuation_cmd_map/brake_map.csv + + convert_accel_cmd: true + convert_brake_cmd: true + + convert_steer_cmd_method: "vgr" + vgr_coef_a: 15.713 + vgr_coef_b: 0.053 + vgr_coef_c: 0.042 + enable_pub_steer: true + + mechanical_params: + kp: 386.9151820510161 + ki: 5.460970982628869 + kd: 0.03550834077602694 + ff_gain: 0.03051963576179274 + angle_limit: 10.0 + rate_limit: 3.0 + dead_zone_threshold: 0.00708241866710033 + poly_a: 0.15251276182076065 + poly_b: -0.17301900674117585 + poly_c: 1.5896528355739639 + poly_d: 0.002300899817071436 + poly_e: -0.0418928856764797 + poly_f: 0.18449047960081838 + poly_g: -0.06320887302605509 + poly_h: 0.18696796150634806 + inertia: 25.17844747941984 + damping: 117.00653795106054 + stiffness: 0.17526182368541224 + friction: 0.6596571248682918 + vgr_coef_a: 2.4181735349544224 + vgr_coef_b: -0.013434076966833082 + vgr_coef_c: -0.033963661615283594 + steering_torque_limit: 30.0 + torque_delay_time: 0.0007641271506616108 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 96cfa1587e646..685708844d32d 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 @@ -21,6 +21,7 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" #include #include @@ -147,7 +148,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt // Initial value must be set to current_input_command_ with the correct type. // If not, the vehicle_model will not be updated, and it will die when publishing the state. const auto vehicle_model_type_str = declare_parameter("vehicle_model_type", "IDEAL_STEER_VEL"); - if (vehicle_model_type_str == "ACTUATION_CMD") { + if (vehicle_model_type_str.find("ACTUATION_CMD") != std::string::npos) { current_input_command_ = ActuationCommandStamped(); sub_actuation_cmd_ = create_subscription( "input/actuation_command", QoS{1}, @@ -311,9 +312,7 @@ void SimplePlanningSimulator::initialize_vehicle_model(const std::string & vehic vehicle_model_ptr_ = std::make_shared( timer_sampling_time_ms_ / 1000.0, model_module_paths, model_param_paths, model_class_names); - } else if (vehicle_model_type_str == "ACTUATION_CMD") { - vehicle_model_type_ = VehicleModelType::ACTUATION_CMD; - + } else if (vehicle_model_type_str.find("ACTUATION_CMD") != std::string::npos) { // time delay const double accel_time_delay = declare_parameter("accel_time_delay"); const double accel_time_constant = declare_parameter("accel_time_constant"); @@ -323,37 +322,77 @@ void SimplePlanningSimulator::initialize_vehicle_model(const std::string & vehic // command conversion flag const bool convert_accel_cmd = declare_parameter("convert_accel_cmd"); const bool convert_brake_cmd = declare_parameter("convert_brake_cmd"); - const bool convert_steer_cmd = declare_parameter("convert_steer_cmd"); // actuation conversion map const std::string accel_map_path = declare_parameter("accel_map_path"); const std::string brake_map_path = declare_parameter("brake_map_path"); - // init vehicle model depending on convert_steer_cmd_method - if (convert_steer_cmd) { - const std::string convert_steer_cmd_method = - declare_parameter("convert_steer_cmd_method"); - if (convert_steer_cmd_method == "vgr") { - const double vgr_coef_a = declare_parameter("vgr_coef_a"); - const double vgr_coef_b = declare_parameter("vgr_coef_b"); - const double vgr_coef_c = declare_parameter("vgr_coef_c"); - vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, - timer_sampling_time_ms_ / 1000.0, accel_time_delay, accel_time_constant, brake_time_delay, - brake_time_constant, steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, - convert_brake_cmd, convert_steer_cmd, accel_map_path, brake_map_path, vgr_coef_a, - vgr_coef_b, vgr_coef_c); - } else if (convert_steer_cmd_method == "steer_map") { - const std::string steer_map_path = declare_parameter("steer_map_path"); - vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, - timer_sampling_time_ms_ / 1000.0, accel_time_delay, accel_time_constant, brake_time_delay, - brake_time_constant, steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, - convert_brake_cmd, convert_steer_cmd, accel_map_path, brake_map_path, steer_map_path); - } else { - throw std::invalid_argument( - "Invalid convert_steer_cmd_method: " + convert_steer_cmd_method); - } + if (vehicle_model_type_str == "ACTUATION_CMD") { + vehicle_model_type_ = VehicleModelType::ACTUATION_CMD; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, accel_time_delay, accel_time_constant, brake_time_delay, + brake_time_constant, steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, + convert_brake_cmd, accel_map_path, brake_map_path); + } else if (vehicle_model_type_str == "ACTUATION_CMD_VGR") { + vehicle_model_type_ = VehicleModelType::ACTUATION_CMD_VGR; + const double vgr_coef_a = declare_parameter("vgr_coef_a"); + const double vgr_coef_b = declare_parameter("vgr_coef_b"); + const double vgr_coef_c = declare_parameter("vgr_coef_c"); + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, accel_time_delay, accel_time_constant, brake_time_delay, + brake_time_constant, steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, + convert_brake_cmd, accel_map_path, brake_map_path, vgr_coef_a, vgr_coef_b, vgr_coef_c); + } else if (vehicle_model_type_str == "ACTUATION_CMD_MECHANICAL") { + vehicle_model_type_ = VehicleModelType::ACTUATION_CMD_MECHANICAL; + const double vgr_coef_a = declare_parameter("vgr_coef_a"); + const double vgr_coef_b = declare_parameter("vgr_coef_b"); + const double vgr_coef_c = declare_parameter("vgr_coef_c"); + + const MechanicalParams mechanical_params = std::invoke([this]() -> MechanicalParams { + const std::string ns = "mechanical_params."; + MechanicalParams p; + p.kp = declare_parameter(ns + "kp"); + p.ki = declare_parameter(ns + "ki"); + p.kd = declare_parameter(ns + "kd"); + p.ff_gain = declare_parameter(ns + "ff_gain"); + p.angle_limit = declare_parameter(ns + "angle_limit"); + p.rate_limit = declare_parameter(ns + "rate_limit"); + p.dead_zone_threshold = declare_parameter(ns + "dead_zone_threshold"); + p.poly_a = declare_parameter(ns + "poly_a"); + p.poly_b = declare_parameter(ns + "poly_b"); + p.poly_c = declare_parameter(ns + "poly_c"); + p.poly_d = declare_parameter(ns + "poly_d"); + p.poly_e = declare_parameter(ns + "poly_e"); + p.poly_f = declare_parameter(ns + "poly_f"); + p.poly_g = declare_parameter(ns + "poly_g"); + p.poly_h = declare_parameter(ns + "poly_h"); + p.inertia = declare_parameter(ns + "inertia"); + p.damping = declare_parameter(ns + "damping"); + p.stiffness = declare_parameter(ns + "stiffness"); + p.friction = declare_parameter(ns + "friction"); + p.steering_torque_limit = declare_parameter(ns + "steering_torque_limit"); + p.torque_delay_time = declare_parameter(ns + "torque_delay_time"); + return p; + }); + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, accel_time_delay, accel_time_constant, brake_time_delay, + brake_time_constant, steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, + convert_brake_cmd, accel_map_path, brake_map_path, vgr_coef_a, vgr_coef_b, vgr_coef_c, + mechanical_params); + } else if (vehicle_model_type_str == "ACTUATION_CMD_STEER_MAP") { + vehicle_model_type_ = VehicleModelType::ACTUATION_CMD_STEER_MAP; + const std::string steer_map_path = declare_parameter("steer_map_path"); + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, accel_time_delay, accel_time_constant, brake_time_delay, + brake_time_constant, steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, + convert_brake_cmd, accel_map_path, brake_map_path, steer_map_path); + } else { + throw std::invalid_argument( + "Invalid ACTUATION_CMD vehicle_model_type: " + vehicle_model_type_str); } } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); @@ -710,7 +749,10 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & 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::ACTUATION_CMD) { + vehicle_model_type_ == VehicleModelType::ACTUATION_CMD || + vehicle_model_type_ == VehicleModelType::ACTUATION_CMD_VGR || + vehicle_model_type_ == VehicleModelType::ACTUATION_CMD_MECHANICAL || + vehicle_model_type_ == VehicleModelType::ACTUATION_CMD_STEER_MAP) { state << x, y, yaw, vx, steer, accx; } vehicle_model_ptr_->setState(state); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp new file mode 100644 index 0000000000000..0977cd9e25a92 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp @@ -0,0 +1,352 @@ +// 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. + +#include "simple_planning_simulator/utils/mechanical_controller.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::simple_planning_simulator::utils::mechanical_controller +{ + +using DelayBuffer = std::deque>; +using DelayOutput = std::pair, DelayBuffer>; + +DelayOutput delay( + const double signal, const double delay_time, const DelayBuffer & buffer, + const double elapsed_time) +{ + DelayBuffer new_buffer = buffer; + + new_buffer.push_back(std::make_pair(signal, elapsed_time)); + + if (!buffer.empty() && (elapsed_time - new_buffer.front().second) >= delay_time) { + const double delayed_signal = new_buffer.front().first; + new_buffer.pop_front(); + return {delayed_signal, new_buffer}; + } else { + return {std::nullopt, new_buffer}; + } +} + +double sign(const double x) +{ + return (x >= 0.0) ? 1.0 : -1.0; +} + +double apply_limits( + const double current_angle, const double previous_angle, const double angle_limit, + const double rate_limit, const double dt) +{ + const double angle_diff = std::clamp(current_angle, -angle_limit, angle_limit) - previous_angle; + const double rate_limited_diff = std::clamp(angle_diff, -rate_limit * dt, rate_limit * dt); + return std::clamp(previous_angle + rate_limited_diff, -angle_limit, angle_limit); +} + +double feedforward(const double input_angle, const double ff_gain) +{ + return ff_gain * input_angle; +} + +double polynomial_transform( + const double torque, const double speed, const double a, const double b, const double c, + const double d, const double e, const double f, const double g, const double h) +{ + return a * torque * torque * torque + b * torque * torque + c * torque + + d * speed * speed * speed + e * speed * speed + f * speed + g * torque * speed + h; +} + +PIDController::PIDController(const double kp, const double ki, const double kd) +: kp_(kp), ki_(ki), kd_(kd), state_{0.0, 0.0} +{ +} + +double PIDController::compute( + const double error, const double integral, const double prev_error, const double dt) const +{ + const double p_term = kp_ * error; + const double i_term = ki_ * integral; + const double d_term = dt < 1e-6 ? 0.0 : kd_ * (error - prev_error) / dt; + + return p_term + i_term + d_term; +} + +void PIDController::update_state(const double error, const double dt) +{ + state_.integral += error * dt; + state_.error = error; +}; + +void PIDController::update_state( + const double k1_error, const double k2_error, const double k3_error, const double k4_error, + const double dt) +{ + state_.error = (k1_error + 2 * k2_error + 2 * k3_error + k4_error) / 6.0; + state_.integral += state_.error * dt; +}; + +PIDControllerState PIDController::get_state() const +{ + return state_; +} + +void PIDController::clear_state() +{ + state_ = {0.0, 0.0}; +} + +SteeringDynamics::SteeringDynamics( + const double angular_position, const double angular_velocity, const double inertia, + const double damping, const double stiffness, const double friction, + const double dead_zone_threshold) +: state_{angular_position, angular_velocity, false}, + inertia_(inertia), + damping_(damping), + stiffness_(stiffness), + friction_(friction), + dead_zone_threshold_(dead_zone_threshold) +{ +} + +bool SteeringDynamics::is_in_dead_zone( + const SteeringDynamicsState & state, const double input_torque) const +{ + bool is_in_dead_zone = state.is_in_dead_zone; + const int rotation_direction = sign(state.angular_velocity); + const int input_direction = sign(input_torque); + + if (input_direction != rotation_direction && std::abs(input_torque) < dead_zone_threshold_) { + return true; + } + + if (is_in_dead_zone) { + return !(dead_zone_threshold_ <= std::abs(input_torque)); + } + + return false; +} + +SteeringDynamicsDeltaState SteeringDynamics::calc_model( + const SteeringDynamicsState & state, const double input_torque) const +{ + const double friction_force = friction_ * sign(state.angular_velocity); + const double angular_acceleration = (input_torque - damping_ * state.angular_velocity - + stiffness_ * state.angular_position - friction_force) / + inertia_; + + const double d_angular_velocity = angular_acceleration; + const double d_angular_position = state.angular_velocity; + + return {d_angular_position, d_angular_velocity}; +} + +void SteeringDynamics::set_state(const SteeringDynamicsState & state) +{ + state_ = state; +} + +SteeringDynamicsState SteeringDynamics::get_state() const +{ + return state_; +} + +void SteeringDynamics::set_steer(const double steer) +{ + state_.angular_position = steer; +} + +void SteeringDynamics::clear_state() +{ + state_ = {0.0, 0.0, false}; +} + +MechanicalController::MechanicalController(const MechanicalParams & params) +: pid_(params.kp, params.ki, params.kd), + steering_dynamics_( + 0.0, 0.0, params.inertia, params.damping, params.stiffness, params.friction, + params.dead_zone_threshold), + params_(params) +{ +} + +void MechanicalController::clear_state() +{ + delay_buffer_.clear(); + pid_.clear_state(); + steering_dynamics_.clear_state(); +} + +void MechanicalController::set_steer(const double steer) +{ + steering_dynamics_.set_steer(steer); +} + +[[maybe_unused]] double MechanicalController::update_euler( + const double input_angle, const double speed, const double prev_input_angle, const double dt) +{ + const auto dynamics_state = steering_dynamics_.get_state(); + + const auto d_state = + run_one_step(input_angle, speed, prev_input_angle, dt, delay_buffer_, pid_, steering_dynamics_); + + const double d_angular_position = d_state.dynamics_d_state.d_angular_position; + const double d_angular_velocity = d_state.dynamics_d_state.d_angular_velocity; + + auto dynamics_state_new = dynamics_state; + dynamics_state_new.angular_position = std::clamp( + dynamics_state.angular_position + d_angular_position * dt, -params_.angle_limit, + params_.angle_limit); + dynamics_state_new.angular_velocity = std::clamp( + dynamics_state.angular_velocity + d_angular_velocity * dt, -params_.rate_limit, + params_.rate_limit); + dynamics_state_new.is_in_dead_zone = d_state.is_in_dead_zone; + steering_dynamics_.set_state(dynamics_state_new); + + pid_.update_state(d_state.pid_error, dt); + delay_buffer_ = d_state.delay_buffer; + + return dynamics_state_new.angular_position; +} + +double MechanicalController::update_runge_kutta( + const double input_angle, const double speed, const double prev_input_angle, const double dt) +{ + const auto dynamics_state = steering_dynamics_.get_state(); + + // NOTE: k1, k2, k3, k4 suffix denote the intermediate points of RK4 + const auto k1 = + run_one_step(input_angle, speed, prev_input_angle, dt, delay_buffer_, pid_, steering_dynamics_); + + auto dynamics_for_k2 = steering_dynamics_; + auto dynamics_state_for_k2 = steering_dynamics_.get_state(); + dynamics_state_for_k2.angular_position = + dynamics_state.angular_position + k1.dynamics_d_state.d_angular_position * 0.5 * dt; + dynamics_state_for_k2.angular_velocity = + dynamics_state.angular_velocity + k1.dynamics_d_state.d_angular_velocity * 0.5 * dt; + dynamics_for_k2.set_state(dynamics_state_for_k2); + const auto k2 = + run_one_step(input_angle, speed, prev_input_angle, dt, delay_buffer_, pid_, dynamics_for_k2); + + auto dynamics_for_k3 = steering_dynamics_; + auto dynamics_state_for_k3 = steering_dynamics_.get_state(); + dynamics_state_for_k3.angular_position = + dynamics_state.angular_position + k2.dynamics_d_state.d_angular_position * 0.5 * dt; + dynamics_state_for_k3.angular_velocity = + dynamics_state.angular_velocity + k2.dynamics_d_state.d_angular_velocity * 0.5 * dt; + dynamics_for_k3.set_state(dynamics_state_for_k3); + const auto k3 = + run_one_step(input_angle, speed, prev_input_angle, dt, delay_buffer_, pid_, dynamics_for_k3); + + auto dynamics_for_k4 = steering_dynamics_; + auto dynamics_state_for_k4 = steering_dynamics_.get_state(); + dynamics_state_for_k4.angular_position = + dynamics_state.angular_position + k3.dynamics_d_state.d_angular_position * dt; + dynamics_state_for_k4.angular_velocity = + dynamics_state.angular_velocity + k3.dynamics_d_state.d_angular_velocity * dt; + dynamics_for_k4.set_state(dynamics_state_for_k4); + const auto k4 = + run_one_step(input_angle, speed, prev_input_angle, dt, delay_buffer_, pid_, dynamics_for_k4); + + const double d_angular_position = + (k1.dynamics_d_state.d_angular_position + 2.0 * k2.dynamics_d_state.d_angular_position + + 2.0 * k3.dynamics_d_state.d_angular_position + k4.dynamics_d_state.d_angular_position) / + 6.0; + const double d_angular_velocity = + (k1.dynamics_d_state.d_angular_velocity + 2.0 * k2.dynamics_d_state.d_angular_velocity + + 2.0 * k3.dynamics_d_state.d_angular_velocity + k4.dynamics_d_state.d_angular_velocity) / + 6.0; + + // update steering dynamics/controller internal state + auto dynamics_state_new = dynamics_state; + dynamics_state_new.angular_position = std::clamp( + dynamics_state.angular_position + d_angular_position * dt, -params_.angle_limit, + params_.angle_limit); + dynamics_state_new.angular_velocity = std::clamp( + dynamics_state.angular_velocity + d_angular_velocity * dt, -params_.rate_limit, + params_.rate_limit); + pid_.update_state(k1.pid_error, k2.pid_error, k3.pid_error, k4.pid_error, dt); + if ( + k1.delay_buffer.empty() || k2.delay_buffer.empty() || k3.delay_buffer.empty() || + k4.delay_buffer.empty()) { + // This condition is assumed to never be met because it is always pushed by + // the delay() function. + return dynamics_state.angular_position; + } + const double delayed_signal = + (k1.delay_buffer.back().first + 2.0 * k2.delay_buffer.back().first + + 2.0 * k3.delay_buffer.back().first + k4.delay_buffer.back().first) / + 6.0; + const double elapsed_time = delay_buffer_.empty() ? dt : delay_buffer_.back().second + dt; + delay_buffer_ = + delay(delayed_signal, params_.torque_delay_time, delay_buffer_, elapsed_time).second; + dynamics_state_new.is_in_dead_zone = + steering_dynamics_.is_in_dead_zone(dynamics_state_new, delayed_signal); + steering_dynamics_.set_state(dynamics_state_new); + + return dynamics_state_new.angular_position; +} + +StepResult MechanicalController::run_one_step( + const double input_angle, const double speed, const double prev_input_angle, const double dt, + const DelayBuffer & delay_buffer, const PIDController & pid, + const SteeringDynamics & dynamics) const +{ + const auto dynamics_state = dynamics.get_state(); + const auto pid_state = pid.get_state(); + + const double limited_input_angle = + apply_limits(input_angle, prev_input_angle, params_.angle_limit, params_.rate_limit, dt); + + const double ff_torque = feedforward(limited_input_angle, params_.ff_gain); + + const double pid_error = limited_input_angle - dynamics_state.angular_position; + + const double pid_torque = + pid.compute(pid_error, pid_state.integral + pid_error * dt, pid_state.error, dt); + + const double total_torque = ff_torque + pid_torque; + + // NOTE: do not distinguish between forward and backward driving + const double steering_torque = std::clamp( + polynomial_transform( + total_torque, std::abs(speed), params_.poly_a, params_.poly_b, params_.poly_c, params_.poly_d, + params_.poly_e, params_.poly_f, params_.poly_g, params_.poly_h), + -params_.steering_torque_limit, params_.steering_torque_limit); + + const double elapsed_time = delay_buffer.empty() ? dt : delay_buffer.back().second + dt; + const auto [delayed_torque_opt, delay_buffer_new] = + delay(steering_torque, params_.torque_delay_time, delay_buffer, elapsed_time); + + if (!delayed_torque_opt.has_value()) { + return {delay_buffer_new, pid_error, {0.0, 0.0}, dynamics_state.is_in_dead_zone}; + } + + const bool is_in_dead_zone = dynamics.is_in_dead_zone(dynamics_state, steering_torque); + if (is_in_dead_zone) { + return {delay_buffer_new, pid_error, {0.0, 0.0}, true}; + } + + const auto d_state = dynamics.calc_model(dynamics.get_state(), steering_torque); + + return {delay_buffer_new, pid_error, d_state, false}; +} + +} // namespace autoware::simple_planning_simulator::utils::mechanical_controller diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index 74ad765687a2e..469c30dc4f0cf 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -108,43 +108,13 @@ double BrakeMap::getBrake(const double acc, double vel) const return autoware::interpolation::lerp(interpolated_acc_vec, brake_indices, acc); } -// steer map sim model +// convert only longitudinal actuation command SimModelActuationCmd::SimModelActuationCmd( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double accel_delay, double accel_time_constant, double brake_delay, double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, - bool convert_accel_cmd, bool convert_brake_cmd, bool convert_steer_cmd, - std::string accel_map_path, std::string brake_map_path, std::string steer_map_path) -: SimModelInterface(6 /* dim x */, 5 /* dim u */), - MIN_TIME_CONSTANT(0.03), - vx_lim_(vx_lim), - vx_rate_lim_(vx_rate_lim), - steer_lim_(steer_lim), - steer_rate_lim_(steer_rate_lim), - wheelbase_(wheelbase), - accel_delay_(accel_delay), - accel_time_constant_(std::max(accel_time_constant, MIN_TIME_CONSTANT)), - brake_delay_(brake_delay), - brake_time_constant_(std::max(brake_time_constant, MIN_TIME_CONSTANT)), - steer_delay_(steer_delay), - steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), - steer_bias_(steer_bias) -{ - initializeInputQueue(dt); - convert_accel_cmd_ = convert_accel_cmd && accel_map_.readActuationMapFromCSV(accel_map_path); - convert_brake_cmd_ = convert_brake_cmd && brake_map_.readActuationMapFromCSV(brake_map_path); - convert_steer_cmd_ = convert_steer_cmd && steer_map_.readActuationMapFromCSV(steer_map_path); - actuation_sim_type_ = ActuationSimType::STEER_MAP; -} - -// VGR sim model -SimModelActuationCmd::SimModelActuationCmd( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double accel_delay, double accel_time_constant, double brake_delay, - double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, - bool convert_accel_cmd, bool convert_brake_cmd, bool convert_steer_cmd, - std::string accel_map_path, std::string brake_map_path, double vgr_coef_a, double vgr_coef_b, - double vgr_coef_c) + bool convert_accel_cmd, bool convert_brake_cmd, std::string accel_map_path, + std::string brake_map_path) : SimModelInterface(6 /* dim x */, 5 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -159,15 +129,12 @@ SimModelActuationCmd::SimModelActuationCmd( steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_bias_(steer_bias), - convert_steer_cmd_(convert_steer_cmd), - vgr_coef_a_(vgr_coef_a), - vgr_coef_b_(vgr_coef_b), - vgr_coef_c_(vgr_coef_c) + convert_accel_cmd_(convert_accel_cmd), + convert_brake_cmd_(convert_brake_cmd) { initializeInputQueue(dt); convert_accel_cmd_ = convert_accel_cmd && accel_map_.readActuationMapFromCSV(accel_map_path); convert_brake_cmd_ = convert_brake_cmd && brake_map_.readActuationMapFromCSV(brake_map_path); - actuation_sim_type_ = ActuationSimType::VGR; } double SimModelActuationCmd::getX() @@ -222,6 +189,7 @@ void SimModelActuationCmd::update(const double & dt) delayed_input(IDX_U::SLOPE_ACCX) = input_(IDX_U::SLOPE_ACCX); const auto prev_state = state_; + updateRungeKutta(dt, delayed_input); // take velocity/steer limit explicitly @@ -248,36 +216,32 @@ void SimModelActuationCmd::initializeInputQueue(const double & dt) std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0); } -Eigen::VectorXd SimModelActuationCmd::calcModel( +Eigen::VectorXd SimModelActuationCmd::calcLongitudinalModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { using autoware_vehicle_msgs::msg::GearCommand; const double vel = std::clamp(state(IDX::VX), -vx_lim_, vx_lim_); const double acc = std::clamp(state(IDX::ACCX), -vx_rate_lim_, vx_rate_lim_); - const double yaw = state(IDX::YAW); const double steer = state(IDX::STEER); - + const double yaw = state(IDX::YAW); const double accel = input(IDX_U::ACCEL_DES); const double brake = input(IDX_U::BRAKE_DES); const auto gear = input(IDX_U::GEAR); - // 1) calculate acceleration by accel and brake command + // calculate acceleration by accel and brake command const double acc_des_wo_slope = std::clamp( std::invoke([&]() -> double { - // Select the non-zero value between accel and brake and calculate the acceleration if (convert_accel_cmd_ && accel > 0.0) { - // convert accel command to acceleration return accel_map_.getControlCommand(accel, vel); } else if (convert_brake_cmd_ && brake > 0.0) { - // convert brake command to acceleration return brake_map_.getControlCommand(brake, vel); } else { - // if conversion is disabled, accel command is directly used as acceleration return accel; } }), -vx_rate_lim_, vx_rate_lim_); + // add slope acceleration considering the gear state const double acc_by_slope = input(IDX_U::SLOPE_ACCX); const double acc_des = std::invoke([&]() -> double { @@ -290,36 +254,31 @@ Eigen::VectorXd SimModelActuationCmd::calcModel( }); const double acc_time_constant = accel > 0.0 ? accel_time_constant_ : brake_time_constant_; - // 2) calculate steering rate by steer command - const double steer_rate = std::clamp( - std::invoke([&]() -> double { - // if conversion is enabled, calculate steering rate from steer command - if (convert_steer_cmd_) { - if (actuation_sim_type_ == ActuationSimType::VGR) { - // convert steer wheel command to steer rate - const double steer_des = - calculateSteeringTireCommand(vel, steer, input(IDX_U::STEER_DES)); - return -(getSteer() - steer_des) / steer_time_constant_; - } else if (actuation_sim_type_ == ActuationSimType::STEER_MAP) { - // convert steer command to steer rate - return steer_map_.getControlCommand(input(IDX_U::STEER_DES), vel) / steer_time_constant_; - } - } - // otherwise, steer command is desired steering angle, so calculate steering rate from the - // difference between the desired steering angle and the current steering angle. - const double steer_des = std::clamp(input(IDX_U::STEER_DES), -steer_lim_, steer_lim_); - return -(getSteer() - steer_des) / steer_time_constant_; - }), - -steer_rate_lim_, steer_rate_lim_); + // calculate derivative of longitudinal states except steering + Eigen::VectorXd d_longitudinal_state = Eigen::VectorXd::Zero(dim_x_); + d_longitudinal_state(IDX::X) = vel * cos(yaw); + d_longitudinal_state(IDX::Y) = vel * sin(yaw); + d_longitudinal_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_longitudinal_state(IDX::VX) = acc; + d_longitudinal_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant; - 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::tan(steer) / wheelbase_; - d_state(IDX::VX) = acc; - d_state(IDX::STEER) = steer_rate; - d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant; + return d_longitudinal_state; +} +double SimModelActuationCmd::calcLateralModel( + [[maybe_unused]] const double steer, const double steer_input, [[maybe_unused]] const double vel) +{ + const double steer_rate = std::clamp( + -(getSteer() - steer_input) / steer_time_constant_, -steer_rate_lim_, steer_rate_lim_); + return steer_rate; +} + +Eigen::VectorXd SimModelActuationCmd::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + Eigen::VectorXd d_state = calcLongitudinalModel(state, input); + d_state(IDX::STEER) = + calcLateralModel(state(IDX::STEER), input(IDX_U::STEER_DES), state(IDX::VX)); return d_state; } @@ -383,20 +342,57 @@ SimModelActuationCmd::getActuationStatus() const actuation_status.status.brake_status = brake_map_.getBrake(acc_state, vel_state); } - if (convert_steer_cmd_) { - if (actuation_sim_type_ == ActuationSimType::VGR) { - actuation_status.status.steer_status = - calculateSteeringWheelState(vel_state, state_(IDX::STEER)); - } - // NOTE: Conversion by steer map is not supported - // else if (actuation_sim_type_ == ActuationSimType::STEER_MAP) {} + return actuation_status; +} + +/* ------ SteerMap model ----- */ +SimModelActuationCmdSteerMap::SimModelActuationCmdSteerMap( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, std::string accel_map_path, + std::string brake_map_path, std::string steer_map_path) +: SimModelActuationCmd( + vx_lim, steer_lim, vx_rate_lim, steer_rate_lim, wheelbase, dt, accel_delay, accel_time_constant, + brake_delay, brake_time_constant, steer_delay, steer_time_constant, steer_bias, + convert_accel_cmd, convert_brake_cmd, accel_map_path, brake_map_path) +{ + initializeInputQueue(dt); + if (!steer_map_.readActuationMapFromCSV(steer_map_path)) { + throw std::runtime_error("Failed to read steer map from " + steer_map_path); } +} - return actuation_status; +double SimModelActuationCmdSteerMap::calcLateralModel( + [[maybe_unused]] const double steer, const double steer_input, const double vel) +{ + const double steer_rate = std::clamp( + steer_map_.getControlCommand(steer_input, vel) / steer_time_constant_, -steer_rate_lim_, + steer_rate_lim_); + + return steer_rate; } +/* ---------------------------------------- */ -/* ------ Functions for VGR sim model ----- */ -double SimModelActuationCmd::calculateSteeringTireCommand( +/* ------ VGR model ----- */ +SimModelActuationCmdVGR::SimModelActuationCmdVGR( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, std::string accel_map_path, + std::string brake_map_path, double vgr_coef_a, double vgr_coef_b, double vgr_coef_c) +: SimModelActuationCmd( + vx_lim, steer_lim, vx_rate_lim, steer_rate_lim, wheelbase, dt, accel_delay, accel_time_constant, + brake_delay, brake_time_constant, steer_delay, steer_time_constant, steer_bias, + convert_accel_cmd, convert_brake_cmd, accel_map_path, brake_map_path), + vgr_coef_a_(vgr_coef_a), + vgr_coef_b_(vgr_coef_b), + vgr_coef_c_(vgr_coef_c) +{ + initializeInputQueue(dt); +} + +double SimModelActuationCmdVGR::calculateSteeringTireCommand( const double vel, const double steer, const double steer_wheel_des) const { // steer_tire_state -> steer_wheel_state @@ -408,17 +404,114 @@ double SimModelActuationCmd::calculateSteeringTireCommand( return steer_wheel_des / adaptive_gear_ratio; } -double SimModelActuationCmd::calculateSteeringWheelState( +double SimModelActuationCmdVGR::calculateSteeringWheelState( const double vel, const double steer_state) const { return (vgr_coef_a_ + vgr_coef_b_ * vel * vel) * steer_state / (1.0 + vgr_coef_c_ * std::abs(steer_state)); } -double SimModelActuationCmd::calculateVariableGearRatio( +double SimModelActuationCmdVGR::calculateVariableGearRatio( const double vel, const double steer_wheel) const { return std::max( 1e-5, vgr_coef_a_ + vgr_coef_b_ * vel * vel - vgr_coef_c_ * std::fabs(steer_wheel)); } + +double SimModelActuationCmdVGR::calcLateralModel( + const double steer, const double steer_input, const double vel) +{ + const double steer_des = calculateSteeringTireCommand(vel, steer, steer_input); + const double steer_rate = + std::clamp(-(getSteer() - steer_des) / steer_time_constant_, -steer_rate_lim_, steer_rate_lim_); + return steer_rate; +} +/* ---------------------------------------- */ + +/* ------ MECHANICAL model ----- */ +SimModelActuationCmdMechanical::SimModelActuationCmdMechanical( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, std::string accel_map_path, + std::string brake_map_path, double vgr_coef_a, double vgr_coef_b, double vgr_coef_c, + MechanicalParams mechanical_params) +: SimModelActuationCmdVGR( + vx_lim, steer_lim, vx_rate_lim, steer_rate_lim, wheelbase, dt, accel_delay, accel_time_constant, + brake_delay, brake_time_constant, steer_delay, steer_time_constant, steer_bias, + convert_accel_cmd, convert_brake_cmd, accel_map_path, brake_map_path, vgr_coef_a, vgr_coef_b, + vgr_coef_c), + mechanical_controller_(std::make_unique(mechanical_params)) +{ +} + +void SimModelActuationCmdMechanical::update(const double & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + accel_input_queue_.push_back(input_(IDX_U::ACCEL_DES)); + delayed_input(IDX_U::ACCEL_DES) = accel_input_queue_.front(); + accel_input_queue_.pop_front(); + + brake_input_queue_.push_back(input_(IDX_U::BRAKE_DES)); + delayed_input(IDX_U::BRAKE_DES) = brake_input_queue_.front(); + brake_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(); + + delayed_input(IDX_U::GEAR) = input_(IDX_U::GEAR); + delayed_input(IDX_U::SLOPE_ACCX) = input_(IDX_U::SLOPE_ACCX); + + const auto prev_state = state_; + + updateRungeKuttaWithController(dt, delayed_input); + + // take velocity/steer limit explicitly + state_(IDX::VX) = std::clamp(state_(IDX::VX), -vx_lim_, vx_lim_); + state_(IDX::STEER) = std::clamp(state_(IDX::STEER), -steer_lim_, steer_lim_); + + // consider gear + // update position and velocity first, and then acceleration is calculated naturally + updateStateWithGear(state_, prev_state, gear_, dt); +} + +void SimModelActuationCmdMechanical::updateRungeKuttaWithController( + const double dt, const Eigen::VectorXd & input) +{ + // 1) update longitudinal states + const Eigen::VectorXd k1_longitudinal = calcLongitudinalModel(state_, input); + const Eigen::VectorXd k2_longitudinal = + calcLongitudinalModel(state_ + k1_longitudinal * 0.5 * dt, input); + const Eigen::VectorXd k3_longitudinal = + calcLongitudinalModel(state_ + k2_longitudinal * 0.5 * dt, input); + const Eigen::VectorXd k4_longitudinal = + calcLongitudinalModel(state_ + k3_longitudinal * dt, input); + state_ += + dt / 6.0 * (k1_longitudinal + 2.0 * k2_longitudinal + 2.0 * k3_longitudinal + k4_longitudinal); + + // 2) update lateral states + const double steer = state_(IDX::STEER); + const double steer_des = input(IDX_U::STEER_DES); + const double vel = state_(IDX::VX); + + const double steer_tire_des = calculateSteeringTireCommand(vel, steer, steer_des); + + mechanical_controller_->set_steer(state_(IDX::STEER)); + const double new_steer = + mechanical_controller_->update_runge_kutta(steer_tire_des, vel, prev_steer_tire_des_, dt); + state_(IDX::STEER) = new_steer; + prev_steer_tire_des_ = steer_tire_des; +} + +void SimModelActuationCmdMechanical::setState(const Eigen::VectorXd & state) +{ + // NOTE: This function is intended to overwrite the state by setting the initial pose, etc. In + // that case, it is necessary to reset the pid and steering dynamics states of the mechanical + // controller. + mechanical_controller_->clear_state(); + mechanical_controller_->set_steer(state(IDX::STEER)); + state_ = state; +} /* ---------------------------------------- */ 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 d774617dc77f1..6f2590417e574 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -271,19 +271,11 @@ void declareVehicleInfoParams(rclcpp::NodeOptions & node_options) node_options.append_parameter_override("max_steer_angle", 0.7); } +// NOTE: +// command type and vehicle model type area common params to all vehicle models. +// currently, no vehicle model requires additional parameters. using DefaultParamType = std::tuple; -using ActuationCmdParamType = std::tuple; -using ParamType = std::variant; -std::unordered_map vehicle_model_type_map = { - {"IDEAL_STEER_VEL", typeid(DefaultParamType)}, - {"IDEAL_STEER_ACC", typeid(DefaultParamType)}, - {"IDEAL_STEER_ACC_GEARED", typeid(DefaultParamType)}, - {"DELAY_STEER_VEL", typeid(DefaultParamType)}, - {"DELAY_STEER_ACC", typeid(DefaultParamType)}, - {"DELAY_STEER_ACC_GEARED", typeid(DefaultParamType)}, - {"DELAY_STEER_ACC_GEARED_WO_FALL_GUARD", typeid(DefaultParamType)}, - {"ACTUATION_CMD", typeid(ActuationCmdParamType)}}; - +using ParamType = std::variant; std::pair get_common_params(const ParamType & params) { return std::visit( @@ -304,21 +296,10 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) rclcpp::init(0, nullptr); const auto params = GetParam(); - // common parameters const auto common_params = get_common_params(params); const auto command_type = common_params.first; const auto vehicle_model_type = common_params.second; - // optional parameters - std::optional conversion_type{}; // for ActuationCmdParamType - - // Determine the ParamType corresponding to vehicle_model_type and get the specific parameters. - const auto iter = vehicle_model_type_map.find(vehicle_model_type); - if (iter == vehicle_model_type_map.end()) { - throw std::invalid_argument("Unexpected vehicle_model_type."); - } - if (iter->second == typeid(ActuationCmdParamType)) { - conversion_type = std::get<2>(std::get(params)); - } + std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; rclcpp::NodeOptions node_options; node_options.append_parameter_override("initialize_source", "INITIAL_POSE_TOPIC"); @@ -331,7 +312,6 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) node_options.append_parameter_override("brake_time_constant", 0.2); node_options.append_parameter_override("convert_accel_cmd", true); node_options.append_parameter_override("convert_brake_cmd", true); - node_options.append_parameter_override("convert_steer_cmd", true); const auto share_dir = ament_index_cpp::get_package_share_directory("simple_planning_simulator"); const auto accel_map_path = share_dir + "/test/actuation_cmd_map/accel_map.csv"; const auto brake_map_path = share_dir + "/test/actuation_cmd_map/brake_map.csv"; @@ -342,17 +322,31 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) node_options.append_parameter_override("vgr_coef_a", 15.713); node_options.append_parameter_override("vgr_coef_b", 0.053); node_options.append_parameter_override("vgr_coef_c", 0.042); - if (conversion_type.has_value()) { - std::cout << "\n\n vehicle model = " << vehicle_model_type - << ", conversion_type = " << conversion_type.value() << std::endl - << std::endl; - node_options.append_parameter_override("convert_steer_cmd_method", conversion_type.value()); - } else { - std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; - } + // mechanical parameters + node_options.append_parameter_override("mechanical_params.kp", 386.915); + node_options.append_parameter_override("mechanical_params.ki", 5.461); + node_options.append_parameter_override("mechanical_params.kd", 0.036); + node_options.append_parameter_override("mechanical_params.ff_gain", 0.031); + node_options.append_parameter_override("mechanical_params.angle_limit", 10.0); + node_options.append_parameter_override("mechanical_params.rate_limit", 3.0); + node_options.append_parameter_override("mechanical_params.dead_zone_threshold", 0.007); + node_options.append_parameter_override("mechanical_params.poly_a", 0.153); + node_options.append_parameter_override("mechanical_params.poly_b", -0.173); + node_options.append_parameter_override("mechanical_params.poly_c", 1.590); + node_options.append_parameter_override("mechanical_params.poly_d", 0.002); + node_options.append_parameter_override("mechanical_params.poly_e", -0.042); + node_options.append_parameter_override("mechanical_params.poly_f", 0.184); + node_options.append_parameter_override("mechanical_params.poly_g", -0.063); + node_options.append_parameter_override("mechanical_params.poly_h", 0.187); + node_options.append_parameter_override("mechanical_params.inertia", 25.178); + node_options.append_parameter_override("mechanical_params.damping", 117.007); + node_options.append_parameter_override("mechanical_params.stiffness", 0.175); + node_options.append_parameter_override("mechanical_params.friction", 0.660); + node_options.append_parameter_override("mechanical_params.steering_torque_limit", 30.0); + node_options.append_parameter_override("mechanical_params.torque_delay_time", 0.001); declareVehicleInfoParams(node_options); - const auto sim_node = std::make_shared(node_options); + auto sim_node = std::make_shared(node_options); const auto pub_sub_node = std::make_shared(); @@ -364,7 +358,7 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) // acceleration or braking, and whether it turns left or right, and generate an actuation // command. So do not change the map. If it is necessary, you need to change this parameters as // well. - const double target_steer_actuation = 10.0f; + const double target_steer_actuation = 20.0f; const double target_accel_actuation = 0.5f; // const double target_brake_actuation = 0.5f; // unused for now. @@ -375,10 +369,15 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) const auto t = sim_node->now(); sendCommand(command_type, sim_node, pub_sub_node, t, ackermann_cmd, actuation_cmd); }; + // NOTE: Since the node has a queue, the node needs to be re-created. + auto _restartNode = [&]() { + sim_node.reset(); + sim_node = std::make_shared(node_options); + }; // check pub-sub connections { - size_t expected = 1; + constexpr size_t expected = 1; // actuation or ackermann must be subscribed const auto sub_command_count = (command_type == CommandType::Actuation) @@ -404,6 +403,7 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) // go backward // NOTE: positive acceleration with reverse gear drives the vehicle backward. + _restartNode(); _resetInitialpose(); _sendBwdGear(); _sendCommand( @@ -412,6 +412,7 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) isOnBackward(*(pub_sub_node->current_odom_), init_state); // go forward left + _restartNode(); _resetInitialpose(); _sendFwdGear(); _sendCommand( @@ -421,6 +422,7 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) // go backward right // NOTE: positive acceleration with reverse gear drives the vehicle backward. + _restartNode(); _resetInitialpose(); _sendBwdGear(); _sendCommand( @@ -443,5 +445,9 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple(CommandType::Ackermann, "DELAY_STEER_ACC_GEARED"), std::make_tuple(CommandType::Ackermann, "DELAY_STEER_ACC_GEARED_WO_FALL_GUARD"), /* Actuation type */ - std::make_tuple(CommandType::Actuation, "ACTUATION_CMD", "steer_map"), - std::make_tuple(CommandType::Actuation, "ACTUATION_CMD", "vgr"))); + // NOTE: Just "ACTUATION_CMD" sim model converts only accel/brake actuation commands. The test + // is performed for models that convert all accel/brake/steer commands, so the test for + // accel/brake alone is skipped. + std::make_tuple(CommandType::Actuation, "ACTUATION_CMD_STEER_MAP"), + std::make_tuple(CommandType::Actuation, "ACTUATION_CMD_VGR"), + std::make_tuple(CommandType::Actuation, "ACTUATION_CMD_MECHANICAL")));