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

kosuke55 · 2024-11-12T11:30:32Z

Description

add new simulator model.

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.

system identification result is following. I tested real driving data and compare obeserved and simulated steering angle error (focusing on low steer angle)
Results are mecanical > second order > first order in that order for train and test data.

mechanical model

python3 steering_system_rosbag.py  --csv_file odaiba.csv  -m simulate  -p low_angle_0.01.json
Total Error: 18.8549
Simulation completed in 0.4162 seconds.


{
    "parameters": {
        "kp": 385.32623432169896,
        "ki": 5.470220188865222,
        "kd": 0,
        "ff_gain": 0.030497566164202503,
        "angle_limit": 10.0,
        "rate_limit": 3.0,
        "dead_zone_threshold": 0.007076432767148246,
        "a": 0.17791229486165056,
        "b": -0.17173397419765798,
        "c": 1.5599789889825484,
        "d": 0.0025322564364858176,
        "e": -0.04499096271188731,
        "f": 0.20114575254773887,
        "g": -0.07439510952992096,
        "h": 0.19998353912585695,
        "inertia": 25.17888049060484,
        "damping": 116.69225778407515,
        "stiffness": 0.1758244980428317,
        "friction": 0.6563489952292717,
        "delay_time": 0.0
    },
    "cost": 191.41139399730702
}

First order model (same to current model)

python3 steering_system_rosbag.py  --csv_file odaiba.csv -m simulate -t simple -p ./annealing_simple_20241021_091713/params/simple_low_angle_0.01_223.7725.json
Total Error: 22.3770
Simulation completed in 0.0679 seconds.

{
    "parameters": {
        "steer_time_delay": 0.18919629045102362,
        "steer_time_constant": 0.19977909286592008,
        "steer_dead_band": 0.0003349704841227525
    },
    "cost": 223.7725409755146
}

if tuning for deadzone

second order model (not in simple planning simulator and this PR, but I tested)

$ python3 steering_system_rosbag.py  --csv_file odaiba.csv  -m simulate -t simple_second_order -p second_order_low_angle_0.01_211.7917.json
/home/kosuke55/.local/lib/python3.10/site-packages/matplotlib/projections/__init__.py:63: UserWarning: Unable to import Axes3D. This may be due to multiple versions of Matplotlib being installed (e.g. as a system package and as a pip package). As a result, the 3D projection is not available.
  warnings.warn("Unable to import Axes3D. This may be due to multiple versions of "
params: {'steer_time_delay': 0.0007710570453590305, 'mass': 10.864864924637185, 'damping': 30.190196862802175, 'stiffness': 58.34026384572137, 'steer_dead_band': 0.00030095624595404767}
Running simulation with initial parameters...
straight_error: 21.987723516478454
curve_error: 99.00222563388706
angle_rate_error: 0.03344426919210258
error_between_observed_and_input: 106.79344233642182
error_between_output_and_observed: 139.0445551692286
error_between_output_and_input: 201.27649476784683
Total Error: 21.9877
Simulation completed in 0.0601 seconds.

{
    "parameters": {
        "steer_time_delay": 0.0007710570453590305,
        "mass": 10.864864924637185,
        "damping": 30.190196862802175,
        "stiffness": 58.34026384572137,
        "steer_dead_band": 0.00030095624595404767
    },
    "cost": 211.79172486868381
}

How was this PR tested?

psim

add unit test

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

github-actions · 2024-11-12T11:30:50Z

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

You've checked our contribution guidelines.
Your PR follows our pull request guidelines.
All required CI checks pass before marking the PR ready for review.

codecov · 2024-11-12T11:46:06Z

Codecov Report

Attention: Patch coverage is 87.38170% with 40 lines in your changes missing coverage. Please review.

Project coverage is 29.45%. Comparing base (6a1ddbd) to head (4880939).
Report is 74 commits behind head on main.

Files with missing lines	Patch %	Lines
...planning_simulator/utils/mechanical_controller.cpp	84.57%	23 Missing and 4 partials ⚠️
...nning_simulator/simple_planning_simulator_core.cpp	84.61%	5 Missing and 3 partials ⚠️
...imulator/vehicle_model/sim_model_actuation_cmd.cpp	94.11%	4 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #9300      +/-   ##
==========================================
+ Coverage   29.31%   29.45%   +0.13%     
==========================================
  Files        1334     1338       +4     
  Lines      102800   103074     +274     
  Branches    39984    40066      +82     
==========================================
+ Hits        30136    30357     +221     
- Misses      69797    69848      +51     
- Partials     2867     2869       +2

Flag	Coverage Δ		*Carryforward flag
differential	`27.92% <87.38%> (?)`
total	`29.27% <ø> (-0.04%)`	⬇️	Carriedforward from ed8093d

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚨 Try these New Features:

Flaky Tests Detection - Detect and resolve failed and flaky tests

soblin · 2024-11-13T04:42:50Z

.../simple_planning_simulator/include/simple_planning_simulator/utils/mechanical_controller.hpp

+  DelayBuffer delay_buffer_;
+  PIDController pid_;
+  SteeringDynamics steering_dynamics_;
+  MechanicalParams params_;


Suggested change

MechanicalParams params_;

const MechanicalParams params_;

fixed in update from suggestions

...ator/simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp

soblin · 2024-11-13T05:28:56Z

...ator/simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp

+  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 = k4.is_in_dead_zone;


I was just wondering if I can use k4.is_in_dead_zone here.

I fixed it because I thought it might cause a slight discrepancy in judgment.

4880939

The input torque (delayed_signal) is obtained from the results of k1-k4, and it is determined if it is_deazone when it is input to the current dynamics_state.

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);

soblin · 2024-11-13T05:30:28Z

.../simple_planning_simulator/include/simple_planning_simulator/utils/mechanical_controller.hpp

+  double inertia_{0.0};
+  double damping_{0.0};
+  double stiffness_{0.0};
+  double friction_{0.0};
+  double dead_zone_threshold_{0.0};


Suggested change

double inertia_{0.0};

double damping_{0.0};

double stiffness_{0.0};

double friction_{0.0};

double dead_zone_threshold_{0.0};

const double inertia_;

const double damping_;

const double stiffness_;

const double friction_;

const double dead_zone_threshold_;

fixed in update from suggestions

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> update docs Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

…utowarefoundation#9300) * feat(simple_planning_simulator): add mechanical actuaion sim model Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> update docs Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * update from suggestions Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * calc internal state using RK4 results Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

kosuke55 marked this pull request as ready for review November 12, 2024 11:30

kosuke55 requested review from soblin, maxime-clem, TakaHoribe, tkimura4 and zulfaqar-azmi-t4 as code owners November 12, 2024 11:30

github-actions bot added type:documentation Creating or refining documentation. (auto-assigned) component:simulation Virtual environment setups and simulations. (auto-assigned) labels Nov 12, 2024

kosuke55 mentioned this pull request Nov 12, 2024

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

Closed

kosuke55 added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Nov 12, 2024

kosuke55 assigned TakaHoribe and soblin Nov 12, 2024

kosuke55 force-pushed the feat/steering_simulator branch from f948fcd to 8d3b85b Compare November 12, 2024 11:33

soblin reviewed Nov 13, 2024

View reviewed changes

feat(simple_planning_simulator): add mechanical actuaion sim model

c6c9419

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> update docs Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

kosuke55 force-pushed the feat/steering_simulator branch from cf2d17e to c6c9419 Compare November 14, 2024 08:52

kosuke55 added 2 commits November 14, 2024 17:57

update from suggestions

ed8093d

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

calc internal state using RK4 results

4880939

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

soblin approved these changes Nov 20, 2024

View reviewed changes

kosuke55 merged commit 7a19e8a into autowarefoundation:main Nov 20, 2024
30 of 32 checks passed

kosuke55 deleted the feat/steering_simulator branch November 20, 2024 10:58

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

kosuke55 commented Nov 12, 2024 •

edited

Loading

github-actions bot commented Nov 12, 2024 •

edited

Loading

codecov bot commented Nov 12, 2024 •

edited

Loading

soblin Nov 13, 2024

kosuke55 Nov 14, 2024

soblin Nov 13, 2024 •

edited by kosuke55

Loading

kosuke55 Nov 14, 2024 •

edited

Loading

soblin Nov 13, 2024

kosuke55 Nov 14, 2024

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

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

Conversation

kosuke55 commented Nov 12, 2024 • edited Loading

Description

Related links

How was this PR tested?

Notes for reviewers

Interface changes

Effects on system behavior

github-actions bot commented Nov 12, 2024 • edited Loading

codecov bot commented Nov 12, 2024 • edited Loading

Codecov Report

soblin Nov 13, 2024

Choose a reason for hiding this comment

kosuke55 Nov 14, 2024

Choose a reason for hiding this comment

soblin Nov 13, 2024 • edited by kosuke55 Loading

Choose a reason for hiding this comment

kosuke55 Nov 14, 2024 • edited Loading

Choose a reason for hiding this comment

soblin Nov 13, 2024

Choose a reason for hiding this comment

kosuke55 Nov 14, 2024

Choose a reason for hiding this comment

kosuke55 commented Nov 12, 2024 •

edited

Loading

github-actions bot commented Nov 12, 2024 •

edited

Loading

codecov bot commented Nov 12, 2024 •

edited

Loading

soblin Nov 13, 2024 •

edited by kosuke55

Loading

kosuke55 Nov 14, 2024 •

edited

Loading