[mppi] fix division by zero, clean raw pointers code #5636
Merged
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
leading to cost values being NaNs, which then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high steering at the start of a run Signed-off-by: Adi Vardi <[email protected]>
Signed-off-by: Adi Vardi <[email protected]>
69abc07 to
eb1dec4
Compare
Signed-off-by: Adi Vardi <[email protected]>
0835690 to
f868f6e
Compare
Codecov Report✅ All modified and coverable lines are covered by tests.
... and 3 files with indirect coverage changes 🚀 New features to boost your workflow:
|
jonipol
pushed a commit
to Karelics/navigation2
that referenced
this pull request
Dec 5, 2025
…5636) * [mppi] fix division by zero leading to cost values being NaNs, which then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high steering at the start of a run Signed-off-by: Adi Vardi <[email protected]> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <[email protected]> * fix long line Signed-off-by: Adi Vardi <[email protected]> --------- Signed-off-by: Adi Vardi <[email protected]>
Decwest
pushed a commit
to Decwest/navigation2
that referenced
this pull request
Dec 10, 2025
…5636) * [mppi] fix division by zero leading to cost values being NaNs, which then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high steering at the start of a run Signed-off-by: Adi Vardi <[email protected]> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <[email protected]> * fix long line Signed-off-by: Adi Vardi <[email protected]> --------- Signed-off-by: Adi Vardi <[email protected]>
jonipol
pushed a commit
to Karelics/navigation2
that referenced
this pull request
Dec 11, 2025
…5636) * [mppi] fix division by zero leading to cost values being NaNs, which then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high steering at the start of a run Signed-off-by: Adi Vardi <[email protected]> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <[email protected]> * fix long line Signed-off-by: Adi Vardi <[email protected]> --------- Signed-off-by: Adi Vardi <[email protected]>
jonipol
added a commit
to Karelics/navigation2
that referenced
this pull request
Dec 11, 2025
…ann robots (#2) * Add debug topic to visualize whether MPPI critic has an effect on costs (ros-navigation#5485) * Publish criticsStats Signed-off-by: Tony Najjar <[email protected]> * linting Signed-off-by: Tony Najjar <[email protected]> * change header to stamp Signed-off-by: Tony Najjar <[email protected]> * make unique_pointer Signed-off-by: Tony Najjar <[email protected]> * typo Signed-off-by: Tony Najjar <[email protected]> * Add readme Signed-off-by: Tony Najjar <[email protected]> * add to readme Signed-off-by: Tony Najjar <[email protected]> * fixes Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]> * Make critic stats to work with current branch * [mppi] fix division by zero, clean raw pointers code (ros-navigation#5636) * [mppi] fix division by zero leading to cost values being NaNs, which then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high steering at the start of a run Signed-off-by: Adi Vardi <[email protected]> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <[email protected]> * fix long line Signed-off-by: Adi Vardi <[email protected]> --------- Signed-off-by: Adi Vardi <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]> Signed-off-by: Adi Vardi <[email protected]> Co-authored-by: Tony Najjar <[email protected]> Co-authored-by: Adi Vardi <[email protected]>
Decwest
pushed a commit
to Decwest/navigation2
that referenced
this pull request
Dec 11, 2025
…5636) * [mppi] fix division by zero leading to cost values being NaNs, which then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high steering at the start of a run Signed-off-by: Adi Vardi <[email protected]> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <[email protected]> * fix long line Signed-off-by: Adi Vardi <[email protected]> --------- Signed-off-by: Adi Vardi <[email protected]> Signed-off-by: Decwest <[email protected]>
Alessio-Parmeggiani
added a commit
to Alessio-Parmeggiani/navigation2
that referenced
this pull request
Jan 9, 2026
backporting ros-navigation#5636 to humble_main branch. This leads to nan costs when wz is zero. Signed-off-by: Alessio Parmeggiani <[email protected]>
Alessio-Parmeggiani
added a commit
to Alessio-Parmeggiani/navigation2
that referenced
this pull request
Jan 9, 2026
backporting ros-navigation#5636 to humble_main branch. This leads to nan costs when wz is zero. Signed-off-by: Alessio Parmeggiani <[email protected]>
8 tasks
SteveMacenski
pushed a commit
that referenced
this pull request
Jan 9, 2026
backporting #5636 to humble_main branch. This leads to nan costs when wz is zero. Signed-off-by: Alessio Parmeggiani <[email protected]>
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Basic Info
Description of contribution in a few bullet points
fix division by zero in constraint_critic, leading to cost values being NaNs. These values then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high turning at the start of a run.
Before the change:
https://github.com/user-attachments/assets/d43f7455-e8b7-4bd6-a8b1-12326bd4ce2b
After the change:
https://github.com/user-attachments/assets/31c06d84-6bec-4b57-bbee-d7d33f944515
Also, a minor change in
applyConstraints()to use Eigen Array operations instead of loop and raw pointers. No functional change.Description of documentation updates required from your changes
None
Description of how this change was tested
control_sequence.wzFuture work that may be required in bullet points
None
For Maintainers:
backport-*.