Fix logic with enforcing constrained planning state space

[stephanie-eng]

Description

Fixes #1552, change to logic when enforcing constrained planning state space in the mixed constraint case.

When running the constrained planning tutorial with:
ros2 launch moveit2_tutorials ompl_constrained_planning.launch.py

You should see the following log for the last example (required debug log level):
[moveit.ompl_planning.planning_context_manager]: planning_context_manager: Using OMPL's constrained state space for planning.

[codecov]

Codecov Report

Base: 51.11% // Head: 51.10% // Decreases project coverage by -0.01% ⚠️

Coverage data is based on head (5572a76) compared to base (71e6d90).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1589      +/-   ##
==========================================
- Coverage   51.11%   51.10%   -0.00%     
==========================================
  Files         380      380              
  Lines       31796    31796              
==========================================
- Hits        16249    16246       -3     
- Misses      15547    15550       +3     

Impacted Files	Coverage Δ
...pl/ompl_interface/src/planning_context_manager.cpp	54.37% <0.00%> (ø)
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	75.36% <0.00%> (-1.07%)	⬇️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[sjahr]

Does the tutorial work for you? I tried to test your change but the robot never moved and I got stuck with the first example. Same happened with the last one (after I commented the ones before out) and I did not see the print statement you described above

[stephanie-eng]

The tutorial doesn't work as per #1474 (comment), but Andy has a fix here: moveit/moveit2_tutorials#519

The print statement should appear as DEBUG, so you'll need to set your log level accordingly to see it.

[henningkayser]

I thought we had this fixed a while ago... Backport to Humble, or other versions?