Gpmp2 self collision

[kshaji3]

This PR adds selfCollisionFactor and selfCollisionFactorGP. This PR has contents that were tested on an 18DOF manipulator setup.

• selfCollisionFactor checks at nodes, while selfCollisionFactorGP ensures that the interpolated trajectory is collision-free. Thi

This uses selfCollisionFactor from: https://arxiv.org/pdf/2011.07171, while adding in a custom GP-implementation of it.

[dellaert]

Also changed the base.

[dellaert]

With 5.6 Sol:

I have one performance concern about this factor. Every evaluation computes world positions—and, during linearization, full joint Jacobians—for every query point in RobotQueryPoints, even though the collision factor may reference only a small subset of those points in pairs_. For a detailed whole-robot collision model with sparse collision pairs, this means repeatedly doing transforms and allocating 3 × dof Jacobians that are never used.

The easiest fix may be to compute the unique set of point indices referenced by pairs_ once in the constructor, then evaluate only those points. That likely requires a RobotQueryPoints API that accepts a list of requested point indices, or a collision-specific model containing only the relevant query points. The former is more flexible; the latter is simpler if callers naturally construct one dedicated model per collision factor.

The good news is that the expensive tree discovery and joint-column mapping are already precomputed, and the robot model is shared rather than copied into each factor. The remaining optimization is therefore fairly targeted: replay only the portion of the kinematics needed to produce the referenced points, and avoid building Jacobians for unused points. This becomes increasingly important for many collision spheres, high-DOF robots, GP-interpolated checks, and trajectories with many support intervals.

[dellaert]

This now targets master already, so you should be able to just implement the performance comments above, If they're correct.

[kshaji3]

Pardon my spelling on the commits, but I believe this should have the necessary edits for your performance request.