Planners ompl constraints overconstrain

[BerendPijnenburg]

Description

When creating OMPL constraints the AmbientDimension often gets overconstraint as the coDimension of the constraint is set to 3 even when you constrict less than 3 dimensions.

Current implementation uses the bounds class to never give an error for a dimension if that dimension is set using -1 but still "constrains" that dimension by setting the coDimension of the ompl::base::constraint to 3.
this causes you to be unable to use both a position and orientation constraint with <7 DOF robots as an AmbientDimension of 6 - 3 - 3 = 0 . #2614

Changes

• I removed the derivative function of the bounds class as the sign can already be obtained from the penalty function.

• I Consolidated the BoxConstraint and EqualityConstraint.

• I changed the coDimension of the box and orientation constraints. Now a dimension of a position constraint can be unconstrained if the respective dimension of the constraint region is less than 0 or equal to infinity.

• A dimension of an orientation constraint can be unconstrained if the absolute axis tolerance of the orientation is less than 0 or equal to infinity.

• I removed the init function, it doesn't make sense to have an uninitialized constraint

• I removed i removed the functions: calcError and calcErrorJacobian as their implementation belongs in function and jacobian

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

[jih189]

It works.

[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.

[sjahr]

@BerendPijnenburg Is this still a draft or ready for review?

[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.