GTSAM optimizers + Constraints

[dellaert]

This PR migrates GTDynamics from its internal constrained optimization implementation to GTSAM's native constrained optimization infrastructure.

See copilot's overview below.

[Copilot]

Pull request overview

This PR migrates GTDynamics from its internal constrained optimization implementation to GTSAM's native constrained optimization infrastructure. The changes integrate GTSAM's NonlinearEqualityConstraints, PenaltyOptimizer, and AugmentedLagrangianOptimizer to replace the corresponding GTDynamics classes.

Changes:

• Removed GTDynamics' EqualityConstraint, PenaltyMethodOptimizer, and AugmentedLagrangianOptimizer implementations in favor of GTSAM equivalents
• Added new optimizer infrastructure including HistoryLMOptimizer, QPSolver, SQPOptimizer, and IPOptOptimizer (with optional IFOPT support)
• Updated all tests, examples, and Python bindings to use the new GTSAM-based constraint API
• Added comprehensive CI documentation and consolidated GitHub Actions workflows

Reviewed changes

Copilot reviewed 86 out of 89 changed files in this pull request and generated 4 comments.

Show a summary per file

File	Description
gtdynamics/optimizer/EqualityConstraint.{h,cpp}	Removed - replaced by gtsam::NonlinearEqualityConstraint
gtdynamics/optimizer/PenaltyMethodOptimizer.{h,cpp}	Removed - replaced by gtsam::PenaltyOptimizer
gtdynamics/optimizer/AugmentedLagrangianOptimizer.{h,cpp}	Removed - replaced by gtsam::AugmentedLagrangianOptimizer
gtdynamics/optimizer/Optimizer.cpp	Updated to use GTSAM optimizers via helper function
gtdynamics/constrained_optimizer/*	New directory with base class and advanced optimizers (SQP, IPOpt)
gtdynamics/optimizer/QPSolver.{h,cpp}	New equality-constrained QP solver
gtdynamics/optimizer/HistoryLMOptimizer.h	New LM optimizer that stores iteration history
gtdynamics/manifold/*	Updated to use gtsam::NonlinearEqualityConstraints
tests/*	Updated constraint instantiation to use GTSAM API
examples/*	Updated to use new GTSAM constrained optimization API
CMakeLists.txt	Added IFOPT support and new source subdirectories
README.md	Added constrained optimization documentation and benchmark results
.github/workflows/common-ci.yml	Consolidated CI workflow with C++ and Python testing

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[dellaert]

@copilot open a new pull request to apply changes based on the comments in this thread

[Copilot]

@dellaert I've opened a new pull request, #408, to work on those changes. Once the pull request is ready, I'll request review from you.

[yetongumich]

LGTM.

Currently, we seem to still keep two versions (GTSAM version and GTDynamics version) of ConstrainedOptimizer.h and ConstrainedOptimizationProblem. But it seems necessary to support the unmigrated optimizers in GTDynamics.