feat(simple_planning_simulator): add mechanical actuaion sim model (#…

…9300) * feat(simple_planning_simulator): add mechanical actuaion sim model Signed-off-by: kosuke55 <[email protected]> update docs Signed-off-by: kosuke55 <[email protected]> * update from suggestions Signed-off-by: kosuke55 <[email protected]> * calc internal state using RK4 results Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Nov 20, 2024 · 7a19e8a · 7a19e8a
1 parent 6717fb2
commit 7a19e8a
Show file tree

Hide file tree

Showing 15 changed files with 1,817 additions and 286 deletions.
diff --git 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
diff --git a/simulator/simple_planning_simulator/docs/actuation_cmd_sim.md 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/...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/...e_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<SimModelInterface> vehicle_model_ptr_;  //!< @brief vehicle model pointer
 

diff --git a/...mple_planning_simulator/include/simple_planning_simulator/utils/mechanical_controller.hpp b/...mple_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 <deque>
+#include <map>
+#include <optional>
+#include <string>
+#include <utility>
+
+namespace autoware::simple_planning_simulator::utils::mechanical_controller
+{
+
+using DelayBuffer = std::deque<std::pair<double, double>>;
+using DelayOutput = std::pair<std::optional<double>, 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_