-
Notifications
You must be signed in to change notification settings - Fork 661
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
Conversation
Signed-off-by: Takamasa Horibe <[email protected]>
Signed-off-by: Takamasa Horibe <[email protected]>
Signed-off-by: Takamasa Horibe <[email protected]>
Signed-off-by: Takamasa Horibe <[email protected]>
There was a problem hiding this 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]>
Codecov ReportAttention:
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
*This pull request uses carry forward flags. Click here to find out more. ☔ View full report in Codecov by Sentry. |
@@ -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_; |
There was a problem hiding this comment.
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]>
fixed in e4dadc5 All states are actual values, and the values obtained by |
steer_bias-2024-01-31_15.14.41.mp4 |
Signed-off-by: kosuke55 <[email protected]>
@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_; |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
fixed
c625256
Signed-off-by: kosuke55 <[email protected]>
after c625256
steer_bias_remove_off-2024-02-05_13.12.08.mp4
steer_bias-2024-02-05_13.08.49.mp4steer_bias-2024-02-05_13.09.46.mp4 |
...mulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
Outdated
Show resolved
Hide resolved
...planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp
Outdated
Show resolved
Hide resolved
...planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
Outdated
Show resolved
Hide resolved
…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
…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]>
…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]>
…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]>
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
steer_bias
param in yaml. (e.g. set to 0.01)steering_offset.enable_auto_steering_offset_removal
to false in mpc.param.yamlYou 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.
Post-review checklist for the PR author
The PR author must check the checkboxes below before merging.
After all checkboxes are checked, anyone who has write access can merge the PR.