Adds Sphere Radii to the ObstacleGP factors

[kshaji3]

This PR adds the ability to do sphere-based obstacle avoidance in interpolation by adding it to ObstacleSDFFactorGP. It follows similarly to how this was done in selfCollisionSphereFactor.

There sphere-based obstacle avoidance already exists in ObstacleSDFFactor, so it's not needed to be added there.

[dellaert]

Could you check for copy- paste functionality that should be shared rather than copied?

[Copilot]

Pull request overview

This PR extends GP-interpolated obstacle avoidance (ObstacleSDFFactorGP) to support sphere-based query points by adding a per-query-point radius vector (folded into the standoff distance), consistent with existing sphere handling in other factors. It also centralizes argument validation shared across obstacle SDF factors and updates the wrapper interface and tests accordingly.

Changes:

• Add per-query-point radii support to ObstacleSDFFactorGP (new constructor + error computation uses epsilon + radius).
• Refactor shared validation into validateObstacleSDFFactorArgs(...) and reuse it from both unary and GP obstacle factors.
• Update gtwrap interface (gtdynamics.i) and unit tests to exercise nonzero radii behavior.

Reviewed changes

Copilot reviewed 6 out of 6 changed files in this pull request and generated 2 comments.

Show a summary per file

File	Description
gtdynamics/gpmp2/tests/testObstacleFactors.cpp	Updates tests to pass nonzero radii and validate GP/unary agreement and Jacobians with radii folded into standoff.
gtdynamics/factors/ObstacleSDFFactorGP.h	Adds radii_, a radii-aware constructor, and shared validation call.
gtdynamics/factors/ObstacleSDFFactorGP.cpp	Uses epsilon_ + radii_(i) when evaluating obstacle cost at each query point.
gtdynamics/factors/ObstacleSDFFactor.h	Introduces exported shared validation function and uses it in constructors.
gtdynamics/factors/ObstacleSDFFactor.cpp	Implements shared validation logic for sdf/epsilon/radii consistency.
gtdynamics.i	Exposes the new ObstacleSDFFactorGP constructor with radii and the radii() accessor to the wrapper.

[kshaji3]

I did check for copy-paste functionality, but the sphere requirements here are slightly different for obstacleFactor vs. selfCollisionFactor. I'll check again though, but for now I put the obstacleSDFFactor to share its sphere-checking with the GP- version.

[dellaert]

Karthik, I vibed with 5.6 Sol about my gut feeling about copy paste - it agreed and we came up with the following plan. Please read and evaluate whether you agree, and if so, work with an AI of your choice to plan and execute :-)

The shared radius validation is an improvement. I agree that obstacle avoidance and self-collision have different requirements, so I am not asking you to combine their actual collision calculations.

However, there is still substantial duplication between each regular factor and its GP version. Please consolidate that before merging.

For obstacle avoidance, ObstacleSDFFactor::evaluateError and ObstacleSDFFactorGP::evaluateError both:

• Compute the robot query points.
• Compute epsilon + radius for each point.
• Evaluate hingeLossObstacleCost.
• Assemble the Jacobian with respect to the robot configuration.

Please move this work into one shared function or small class that evaluates the obstacle cost for a given configuration q. Both factors should call that same implementation. The GP factor should only be responsible for interpolating q and converting the resulting configuration Jacobian into Jacobians for its four input variables.

Please make the equivalent change for SelfCollisionSphereFactor and SelfCollisionSphereFactorGP. Their shared implementation should own:

• Querying the referenced robot points.
• Combining the pair’s epsilon with both radii.
• Calling hingeLossSelfCollisionCost.
• Combining the two point Jacobians into the Jacobian with respect to q.

Again, the GP factor should only add interpolation around this shared calculation.

There is also duplicated GP-specific code in the two GP factors: both interpolate q, request a Jacobian with respect to q, and then call updatePoseJacobians. Please put that sequence in a shared helper if it can be done cleanly. The public factor class names can remain unchanged; the goal is to prevent every future GP collision factor from copying this implementation.

Please also address the following while doing the refactor:

• Keep validation helpers as implementation details where possible. Functions such as checkedNrPoints, validateObstacleSDFFactorArgs, and validateAndRestrictSelfCollision do not appear to be useful public APIs. Moving the constructors into .cpp files may make it possible to avoid exporting these helpers.
• Make the obstacle constructors without an explicit radius vector reuse the radius-aware construction path, supplying a checked zero vector. Both currently use robot->nrPoints() directly when initializing that vector, which reintroduces the null-pointer concern raised earlier.
• Preserve the existing factor names and wrapper interface so users still see ObstacleSDFFactor, ObstacleSDFFactorGP, SelfCollisionSphereFactor, and SelfCollisionSphereFactorGP.
• Add or retain tests showing that the regular and GP factors use the same underlying cost calculation at the same configuration, including nonzero radii and Jacobians.

In short, obstacle avoidance and self-collision should remain two separate calculations, but each calculation should have only one implementation. The distinction between the regular and GP factors should be how the configuration is obtained, not a second copy of the collision calculation.

[dellaert]

See comment above

[kshaji3]

I did the cleanups, and also edited the tests. The shared helpers I moved to the ''details'' folder so they wouldn't get exposed publicly. I thought this was the best option for that, but please let me know if there's another method you prefer!

[dellaert]

Three follow-ups on organizing the now-consolidated implementation:

[dellaert]

Amendment: I don't think the header-only factors need to imply that the internal helpers need to be header-only. So I'm fine leaving those as two files.

[dellaert]

Looks great now! Thanks for being so responsive!