GPMP2 Addition

[kshaji3]

Ports over code from GPMP2 that is relevant for manipulator platforms, also updates them to the latest GTSAM standards. This also lays down some of the framework that's needed to eventually incorporate STEAP into the repo as well.
https://github.com/gtrll/gpmp2
Notes:

1. Adds the ability to define obstacle factors, including GP-based ones
2. Adds the ability to define Linear, Lie, and Pose3 GP prior factors, along with their interpolation cousins
3. Also adds joint, velocity factors
4. Enables for path planning around obstacles via GPMP
5. Includes relevant unit tests.

The code was tested in simulation and validated on real hardware (using the Python build)

[Copilot]

Pull request overview

This PR ports GPMP2-style components into GTDynamics to enable GP-based trajectory optimization with obstacle avoidance for manipulator platforms, and wires them into both C++ and Python test suites.

Changes:

• Adds GPMP2-inspired core types (signed distance fields, obstacle cost) and GP interpolators needed to evaluate obstacle costs at support and interpolated states.
• Introduces new obstacle avoidance factors (ObstacleSDFFactor, ObstacleSDFFactorGP) plus GP prior factors (linear + Lie/Pose3) consistent with updated GTSAM patterns.
• Adds C++ and Python unit tests and integrates new test targets into CMake, while exposing the new APIs through gtdynamics.i.

Reviewed changes

Copilot reviewed 24 out of 24 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
python/tests/test_gpmp2_planning.py	Python regression tests for SDF construction + obstacle-avoiding planning around a sphere.
gtdynamics/utils/utils.cpp	Tightens getQc behavior when passed non-Gaussian noise models.
gtdynamics/gpmp2/tests/testObstacleFactors.cpp	End-to-end C++ tests for SDF interpolation, obstacle cost, and obstacle-avoiding trajectory planning.
gtdynamics/gpmp2/tests/testGPInterpolators.cpp	C++ tests for GP linear/Lie/Pose3 interpolators and their Jacobians.
gtdynamics/gpmp2/tests/CMakeLists.txt	Adds gpmp2 test target discovery via gtsamAddTestsGlob.
gtdynamics/gpmp2/SignedDistanceField.h	Implements a trilinear-interpolated SDF + constructor from (positions, distances).
gtdynamics/gpmp2/SDFexception.h	Adds an SDF out-of-range exception type.
gtdynamics/gpmp2/RobotQueryPoints.h	Adds query-point FK + Jacobians for obstacle checking on robots.
gtdynamics/gpmp2/ObstacleCost.h	Adds hinge-loss obstacle cost functions over SDFs (world + frame-attached variants).
gtdynamics/gpmp2/GPutils.h	Adds white-noise-on-acceleration GP utility matrices (Phi/Q/Lambda/Psi).
gtdynamics/gpmp2/GPPose3Interpolator.h	Defines Pose3 GP interpolator alias (via Lie interpolator).
gtdynamics/gpmp2/GPLinearInterpolator.h	Adds a linear GP interpolator and Jacobian propagation helper.
gtdynamics/gpmp2/GPLieInterpolator.h	Adds a generic Lie-group GP interpolator with dimension checks.
gtdynamics/gpmp2/CMakeLists.txt	Integrates gpmp2 subdirectory tests into the build when enabled.
gtdynamics/factors/tests/testGPPriorFactors.cpp	Adds tests for GP prior factors (linear + Lie/Pose3) including Jacobians.
gtdynamics/factors/tests/CMakeLists.txt	Adds factors test target discovery via gtsamAddTestsGlob.
gtdynamics/factors/ObstacleSDFFactorGP.h	Adds obstacle factor evaluated at GP-interpolated states.
gtdynamics/factors/ObstacleSDFFactor.h	Adds unary obstacle factor evaluated at support states.
gtdynamics/factors/GPPose3PriorFactor.h	Adds Pose3 GP prior factor alias (via Lie prior).
gtdynamics/factors/GPLinearPriorFactor.h	Adds linear GP prior factor implementation.
gtdynamics/factors/GPLiePriorFactor.h	Adds generic Lie-group GP prior factor implementation.
gtdynamics/factors/CMakeLists.txt	Integrates factors tests into the build when enabled.
gtdynamics/CMakeLists.txt	Adds gpmp2 to the library source subdirectories.
gtdynamics.i	Exposes new gpmp2 types/factors to the wrapper interface.