Add Constraints to ComputeIK

[VideoSystemsTech]

This adds Constraints to the ComputeIK stage, similar to what is done in MoveIt's PositionIKRequest service.
This way we can filter out IK solutions before they are discarded anyway during planning.

[codecov]

Codecov Report

Patch coverage: 100.00% and project coverage change: +0.06 🎉

Comparison is base (6a01550) 58.71% compared to head (06d0d19) 58.77%.

❗ Current head 06d0d19 differs from pull request most recent head a7f89b4. Consider uploading reports for the commit a7f89b4 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #464      +/-   ##
==========================================
+ Coverage   58.71%   58.77%   +0.06%     
==========================================
  Files          90       90              
  Lines        8459     8471      +12     
==========================================
+ Hits         4966     4978      +12     
  Misses       3493     3493              

Impacted Files	Coverage Δ
core/src/stages/compute_ik.cpp	81.92% <100.00%> (+0.84%)	⬆️

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

[rhaschke]

This is redundant to line 384.

[rhaschke]

Thanks for this suggestion. I'm not fully convinced though. IMHO it doesn't make sense to define additional Cartesian constraints - all constraints are already fully defined by the Cartesian goal pose. Joint constraints though might be useful.
Rejected solutions should include the reason in the comments, distinguishing between collisions and constraints (as well as naming the failed constraints).

[VideoSystemsTech]

Thanks for this suggestion. I'm not fully convinced though. IMHO it doesn't make sense to define additional Cartesian constraints - all constraints are already fully defined by the Cartesian goal pose. Joint constraints though might be useful. Rejected solutions should include the reason in the comments, distinguishing between collisions and constraints (as well as naming the failed constraints).

I agree, Position and Orientation constraints shouldn't be needed, but I also think they might be useful as a further check of constraints. For example, planning for a pose which is outside global position/orientation constraints.

BTW, I cannot find a way to list which constraints were satisfied or not.

[codecov-commenter]

Codecov Report

Attention: Patch coverage is 76.47059% with 4 lines in your changes are missing coverage. Please review.

Project coverage is 55.35%. Comparing base (6b0f2c8) to head (ce13fb9).
Report is 16 commits behind head on master.

Files	Patch %	Lines
core/src/stages/compute_ik.cpp	71.43%	4 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #464      +/-   ##
==========================================
- Coverage   58.82%   55.35%   -3.47%     
==========================================
  Files          91      131      +40     
  Lines        8623    10938    +2315     
  Branches        0      923     +923     
==========================================
+ Hits         5072     6054     +982     
- Misses       3551     4838    +1287     
- Partials        0       46      +46     

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