Fix penalty calculation in constraints cost function in STOMP

[patrickKXMD]

Description

@henningkayser
I mentioned this issue here: #2554 (comment)
I can't plan in STOMP with joint constrants.

I think the penalty result for constraints cost function should be 0.0 when ConstraintEvaluationResult.satisfied is true

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 50.63%. Comparing base (0e1e376) to head (ba370bf).
Report is 46 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2631      +/-   ##
==========================================
- Coverage   50.99%   50.63%   -0.36%     
==========================================
  Files         387      386       -1     
  Lines       32308    32133     -175     
==========================================
- Hits        16473    16267     -206     
- Misses      15835    15866      +31     

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

[henningkayser]

I am a little bit conflicted about this change. I agree, that states that satisfy the constraints should not render states or trajectories invalid. At the same time, it would be great if we could still continue optimizing for constraints within the space. This change would probably result in many solutions that just barely satisfy the constraints since they stop optimizing at the constraint boundaries. Considering, that the current cost function categorizes non-zero costs as invalid, I'll approve this change. But at the same time I would be very interested in follow-ups that would support explicit cost thresholding to catch this case instead.
Can you add a NOTE referencing this issue? #2658

[github-actions]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.