Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(simple_planning_simulator): fix steering bias model #6240

Merged
merged 12 commits into from
Feb 5, 2024

Conversation

TakaHoribe
Copy link
Contributor

@TakaHoribe TakaHoribe commented Jan 30, 2024

Description

In the old implementation, steering_bias was just added in the measurement, thus the behavior was "the vehicle yaw dynamics totally follow the steering command, but the steering output seems different". However, the expected behavior is "the steering output seems to be ideal, but the vehicle yaw dynamics does not match its steering angle". This PR fixes the behavior.

Related links

None

Tests performed

  1. Add steer_bias param in yaml. (e.g. set to 0.01)
  2. Set steering_offset.enable_auto_steering_offset_removal to false in mpc.param.yaml
  3. Run psim
  4. See the vehicle trajectory following is slightly biased to the right/left.

You can also check the steering offset topic /control/trajectory_follower/controller_node_exe/output/estimated_steer_offset converges to the bias value if you set the offset_removal true.

Notes for reviewers

None

Interface changes

None

Effects on system behavior

None by default.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

  • The PR follows the pull request guidelines.
  • The PR has been properly tested.
  • The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

  • There are no open discussions or they are tracked via tickets.
  • The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

@TakaHoribe TakaHoribe requested a review from tkimura4 as a code owner January 30, 2024 16:01
@TakaHoribe TakaHoribe requested a review from kosuke55 January 30, 2024 16:01
@TakaHoribe TakaHoribe added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Jan 30, 2024
@github-actions github-actions bot added type:documentation Creating or refining documentation. (auto-assigned) component:simulation Virtual environment setups and simulations. (auto-assigned) labels Jan 30, 2024
Signed-off-by: Takamasa Horibe <[email protected]>
Copy link
Contributor

@tkimura4 tkimura4 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM

Signed-off-by: Takamasa Horibe <[email protected]>
@TakaHoribe TakaHoribe enabled auto-merge (squash) January 30, 2024 16:25
@TakaHoribe TakaHoribe disabled auto-merge January 30, 2024 23:29
Copy link

codecov bot commented Jan 31, 2024

Codecov Report

Attention: 6 lines in your changes are missing coverage. Please review.

Comparison is base (0d10e81) 14.91% compared to head (8101055) 14.45%.
Report is 7 commits behind head on main.

Files Patch % Lines
...cle_model/sim_model_delay_steer_map_acc_geared.cpp 0.00% 4 Missing ⚠️
...nning_simulator/simple_planning_simulator_core.cpp 0.00% 0 Missing and 1 partial ⚠️
...ulator/vehicle_model/sim_model_delay_steer_vel.cpp 80.00% 0 Missing and 1 partial ⚠️
Additional details and impacted files
@@            Coverage Diff             @@
##             main    #6240      +/-   ##
==========================================
- Coverage   14.91%   14.45%   -0.47%     
==========================================
  Files        1817     1906      +89     
  Lines      125357   129364    +4007     
  Branches    37640    37363     -277     
==========================================
- Hits        18697    18694       -3     
- Misses      85651    89779    +4128     
+ Partials    21009    20891     -118     
Flag Coverage Δ *Carryforward flag
differential 47.23% <66.66%> (?)
total 14.43% <ø> (-0.48%) ⬇️ Carriedforward from c625256

*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.

@@ -127,7 +128,7 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel(
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::YAW) = vel * std::tan(steer + steer_bias_) / wheelbase_;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Only yaw among the states has a biased value. each of the states should be unified as to whether it is an actual value or a sensor value.

Signed-off-by: kosuke55 <[email protected]>
@kosuke55
Copy link
Contributor

Only yaw among the states has a biased value. each of the states should be unified as to whether it is an actual value or a sensor value.

fixed in e4dadc5

All states are actual values, and the values obtained by getSteer() are biased and used in the calculation of the steer_rate in calcModel() and published as an output report.

@kosuke55
Copy link
Contributor

steer_bias-2024-01-31_15.14.41.mp4

Signed-off-by: kosuke55 <[email protected]>
@kosuke55
Copy link
Contributor

@TakaHoribe could you please check my commits? 🙏

@@ -67,11 +68,12 @@ double SimModelDelaySteerAccGeared::getAx()
}
double SimModelDelaySteerAccGeared::getWz()
{
return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_;
return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_;
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@kosuke55 You can remove this change.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed
c625256

@kosuke55
Copy link
Contributor

kosuke55 commented Feb 5, 2024

@TakaHoribe

after c625256
works well!

Add steer_bias param in yaml. (e.g. set to 0.01)
Set steering_offset.enable_auto_steering_offset_removal to false in mpc.param.yaml
Run psim
See the vehicle trajectory following is slightly biased to the right/left.

steer_bias_remove_off-2024-02-05_13.12.08.mp4

You can also check the steering offset topic /control/trajectory_follower/controller_node_exe/output/estimated_steer_offset converges to the bias value if you set the offset_removal true.

steer_bias-2024-02-05_13.08.49.mp4
steer_bias-2024-02-05_13.09.46.mp4

…tor/vehicle_model/sim_model_delay_steer_acc.cpp
…tor/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
…tor/vehicle_model/sim_model_delay_steer_vel.cpp
@TakaHoribe TakaHoribe enabled auto-merge (squash) February 5, 2024 14:52
@TakaHoribe TakaHoribe merged commit 0b90a97 into autowarefoundation:main Feb 5, 2024
21 of 23 checks passed
@TakaHoribe TakaHoribe deleted the fix-steer-bias branch February 5, 2024 15:17
zulfaqar-azmi-t4 pushed a commit to zulfaqar-azmi-t4/autoware.universe that referenced this pull request Feb 6, 2024
…dation#6240)

* fix(simple_planning_simulator): fix steering bias model

Signed-off-by: Takamasa Horibe <[email protected]>

* remove old implementation

Signed-off-by: Takamasa Horibe <[email protected]>

* fix initialize order

Signed-off-by: Takamasa Horibe <[email protected]>

* fix yawrate measurement

Signed-off-by: Takamasa Horibe <[email protected]>

* remove unused code

Signed-off-by: Takamasa Horibe <[email protected]>

* add bias to steer rate

Signed-off-by: kosuke55 <[email protected]>

* add comments

Signed-off-by: kosuke55 <[email protected]>

* fix getWz()

Signed-off-by: kosuke55 <[email protected]>

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp

---------

Signed-off-by: Takamasa Horibe <[email protected]>
Signed-off-by: kosuke55 <[email protected]>
Co-authored-by: kosuke55 <[email protected]>
anhnv3991 pushed a commit to anhnv3991/autoware.universe that referenced this pull request Feb 13, 2024
…dation#6240)

* fix(simple_planning_simulator): fix steering bias model

Signed-off-by: Takamasa Horibe <[email protected]>

* remove old implementation

Signed-off-by: Takamasa Horibe <[email protected]>

* fix initialize order

Signed-off-by: Takamasa Horibe <[email protected]>

* fix yawrate measurement

Signed-off-by: Takamasa Horibe <[email protected]>

* remove unused code

Signed-off-by: Takamasa Horibe <[email protected]>

* add bias to steer rate

Signed-off-by: kosuke55 <[email protected]>

* add comments

Signed-off-by: kosuke55 <[email protected]>

* fix getWz()

Signed-off-by: kosuke55 <[email protected]>

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp

---------

Signed-off-by: Takamasa Horibe <[email protected]>
Signed-off-by: kosuke55 <[email protected]>
Co-authored-by: kosuke55 <[email protected]>
karishma1911 pushed a commit to Interplai/autoware.universe that referenced this pull request Jun 3, 2024
…dation#6240)

* fix(simple_planning_simulator): fix steering bias model

Signed-off-by: Takamasa Horibe <[email protected]>

* remove old implementation

Signed-off-by: Takamasa Horibe <[email protected]>

* fix initialize order

Signed-off-by: Takamasa Horibe <[email protected]>

* fix yawrate measurement

Signed-off-by: Takamasa Horibe <[email protected]>

* remove unused code

Signed-off-by: Takamasa Horibe <[email protected]>

* add bias to steer rate

Signed-off-by: kosuke55 <[email protected]>

* add comments

Signed-off-by: kosuke55 <[email protected]>

* fix getWz()

Signed-off-by: kosuke55 <[email protected]>

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp

---------

Signed-off-by: Takamasa Horibe <[email protected]>
Signed-off-by: kosuke55 <[email protected]>
Co-authored-by: kosuke55 <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:simulation Virtual environment setups and simulations. (auto-assigned) run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) type:documentation Creating or refining documentation. (auto-assigned)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants