[mppi] fix division by zero, clean raw pointers code#5636
Merged
SteveMacenski merged 3 commits intoros-navigation:mainfrom Oct 22, 2025
Merged
[mppi] fix division by zero, clean raw pointers code#5636SteveMacenski merged 3 commits intoros-navigation:mainfrom
SteveMacenski merged 3 commits intoros-navigation:mainfrom
Conversation
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 <adi.vardi@enway.ai>
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
69abc07 to
eb1dec4
Compare
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
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 <adi.vardi@enway.ai> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * fix long line Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
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 <adi.vardi@enway.ai> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * fix long line Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
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 <adi.vardi@enway.ai> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * fix long line Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
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 <tony.najjar.1997@gmail.com> * linting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * change header to stamp Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * make unique_pointer Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * typo Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add readme Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add to readme Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * 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 <adi.vardi@enway.ai> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * fix long line Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Adi Vardi <57910756+adivardi@users.noreply.github.com>
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 <adi.vardi@enway.ai> * [mppi] clean ackermann constraints Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * fix long line Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: Decwest <fumiyaonishi1016@gmail.com>
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 <parmeggiani.alessio1@gmail.com>
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 <parmeggiani.alessio1@gmail.com>
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 <parmeggiani.alessio1@gmail.com>
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
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-*.