diff --git a/dart/math/lcp/README.md b/dart/math/lcp/README.md new file mode 100644 index 0000000000000..2e9669762056f --- /dev/null +++ b/dart/math/lcp/README.md @@ -0,0 +1,396 @@ +# LCP Solver Architecture + +This directory contains the Linear Complementarity Problem (LCP) solver infrastructure for DART. + +## Directory Structure + +``` +lcp/ +├── Types.hpp/cpp # Common types (LCPOptions, LCPResult, SolverStatus) +├── Solver.hpp/cpp # Base LCPSolver interface class +├── All.hpp # Convenience header including all solvers +│ +├── Lemke.hpp/cpp # Legacy Lemke solver (backward compatibility) +├── ODELCPSolver.hpp/cpp # Legacy ODE-based solver (backward compatibility) +│ +├── dantzig/ # Dantzig principal pivoting method (from ODE) +│ ├── Lcp.hpp/cpp +│ ├── Matrix.hpp +│ └── ... +│ +├── pivoting/ # Pivoting methods +│ └── LemkeSolver.hpp/cpp # Modern Lemke wrapper +│ +├── projection/ # Projection/sweeping methods (future) +│ ├── PGSSolver.hpp/cpp # Projected Gauss-Seidel (planned) +│ ├── PSORSolver.hpp/cpp # Projected SOR (planned) +│ └── ... +│ +├── newton/ # Newton methods (future) +│ ├── MinimumMapSolver.hpp # Minimum map Newton (planned) +│ └── ... +│ +└── other/ # Other methods (future) + └── ... +``` + +## Architecture + +### Version 1 (Legacy API - Backward Compatibility) + +The legacy API is preserved for backward compatibility: + +```cpp +// Legacy Lemke solver +#include +int exitCode = dart::math::Lemke(M, q, &z); +bool valid = dart::math::validate(M, z, q); + +// Legacy Dantzig solver +#include +bool success = dart::math::SolveLCP(n, A, x, b, w, nub, lo, hi, findex); + +// Legacy ODE solver +#include +dart::math::ODELCPSolver solver; +solver.Solve(A, b, &x, numContacts, mu, numDir, true); +``` + +### Version 2 (Modern API - Recommended for New Code) + +The new API provides a consistent interface across all solvers: + +```cpp +#include +#include + +// Create solver +auto solver = std::make_shared(); + +// Configure options +dart::math::LCPOptions options; +options.validateSolution = true; +options.absoluteTolerance = 1e-8; + +// Solve +Eigen::VectorXd x(n); +dart::math::LCPResult result = solver->solve(A, b, x, options); + +// Check result +if (result.succeeded()) { + std::cout << "Solution found in " << result.iterations << " iterations\n"; + std::cout << "Residual: " << result.residual << "\n"; + std::cout << "Complementarity error: " << result.complementarity << "\n"; +} else { + std::cerr << "Solver failed: " << dart::math::toString(result.status) << "\n"; + std::cerr << result.message << "\n"; +} +``` + +## Common Types + +### SolverStatus + +Exit status codes for solvers: + +```cpp +enum class SolverStatus { + Success, // Solution found successfully + Failed, // Failed to find a solution + MaxIterations, // Maximum iterations reached + NumericalError, // Numerical issues (NaN, Inf, etc.) + InvalidProblem, // Invalid problem formulation + Degenerate, // Solution is degenerate + NotSolved // Not yet solved +}; +``` + +### LCPResult + +Result structure returned by all solvers: + +```cpp +struct LCPResult { + SolverStatus status; // Exit status + int iterations; // Number of iterations performed + double residual; // Final residual norm + double complementarity; // Complementarity error: ||x * (Ax + b)|| + bool validated; // Whether solution was validated + std::string message; // Additional information + + bool succeeded() const; // Check if solution was successful +}; +``` + +### LCPOptions + +Configuration options for solvers: + +```cpp +struct LCPOptions { + int maxIterations; // Maximum iterations (0 = solver default) + double absoluteTolerance; // Absolute convergence tolerance + double relativeTolerance; // Relative convergence tolerance + double complementarityTolerance; // Complementarity tolerance + bool validateSolution; // Validate solution after solving + double relaxation; // Relaxation parameter (for SOR) + bool warmStart; // Use warm starting + bool earlyTermination; // Early termination (for pivoting) + void* customOptions; // Solver-specific options + + // Factory methods + static LCPOptions withRelaxation(double relaxation, int maxIter = 100); + static LCPOptions highAccuracy(); + static LCPOptions realTime(); +}; +``` + +## Base Solver Interface + +All modern solvers inherit from `LCPSolver`: + +```cpp +class LCPSolver { +public: + virtual ~LCPSolver() = default; + + // Solve with default options + virtual LCPResult solve( + const Eigen::MatrixXd& A, + const Eigen::VectorXd& b, + Eigen::VectorXd& x); + + // Solve with custom options + virtual LCPResult solve( + const Eigen::MatrixXd& A, + const Eigen::VectorXd& b, + Eigen::VectorXd& x, + const LCPOptions& options) = 0; + + virtual std::string getName() const = 0; + virtual std::string getCategory() const = 0; + + virtual LCPOptions getDefaultOptions() const; + virtual void setDefaultOptions(const LCPOptions& options); + +protected: + LCPOptions defaultOptions_; +}; +``` + +## Implemented Solvers + +### Pivoting Methods + +#### LemkeSolver (dart::math::LemkeSolver) + +Lemke's complementary pivot algorithm for standard LCP. + +**Properties**: + +- Algorithm: Lemke's complementary pivoting with artificial variable +- Time: O(n^4) worst case +- Space: O(n^2) +- Convergence: Exact solution (finite termination) +- Matrix Requirements: None (general LCP) +- Named after: Carlton E. Lemke + +**Usage**: + +```cpp +#include + +auto solver = std::make_shared(); +LcpResult result = solver->solve(A, b, x); +``` + +#### Dantzig Principal Pivoting (dart::math::SolveLCP template function) + +Dantzig-Cottle principal pivoting method for BLCP (Boxed LCP) with friction support. + +**Properties**: + +- Algorithm: Principal pivoting method +- Time: O(n^4) worst case +- Space: O(n^2) +- Supports: Bounded variables, friction cones +- Source: Open Dynamics Engine (ODE) +- Named after: George B. Dantzig + +**Usage**: + +```cpp +#include + +bool success = dart::math::SolveLCP( + n, A, x, b, w, nub, lo, hi, findex, earlyTermination); +``` + +### Utility Classes + +#### ODELCPSolver (dart::math::ODELCPSolver) + +Convenience wrapper for contact mechanics problems. Uses Dantzig principal pivoting internally. + +**Usage**: + +```cpp +#include + +dart::math::ODELCPSolver solver; +bool success = solver.Solve(A, b, &x, numContacts, mu, numDir, true); +``` + +## Planned Solvers + +### Projection Methods (projection/) + +- **PGSSolver**: Projected Gauss-Seidel (O(n) per iteration) +- **PSORSolver**: Projected SOR with relaxation parameter +- **BGSSolver**: Blocked Gauss-Seidel for contact problems +- **NNCGSolver**: Nonsmooth Nonlinear Conjugate Gradient + +### Newton Methods (newton/) + +- **MinimumMapSolver**: Minimum map Newton method +- **FischerBurmeisterSolver**: Fischer-Burmeister Newton method + +### Other Methods (other/) + +- **InteriorPointSolver**: Interior point method +- **StaggeringSolver**: Staggering for coupled sub-problems + +## Migration Guide + +### From Legacy Lemke to Modern API + +**Before (v1)**: + +```cpp +#include + +Eigen::VectorXd z(n); +int exitCode = dart::math::Lemke(M, q, &z); +if (exitCode == 0) { + bool valid = dart::math::validate(M, z, q); + // Use solution... +} +``` + +**After (v2)**: + +```cpp +#include + +auto solver = std::make_shared(); +Eigen::VectorXd z(n); +auto result = solver->solve(M, q, z); +if (result.succeeded() && result.validated) { + // Use solution... +} +``` + +### From Legacy Dantzig to Modern API + +The Dantzig principal pivoting solver currently only has a template function API. + +```cpp +// Current (v1) - still recommended for BLCP +#include +bool success = dart::math::SolveLCP(n, A, x, b, w, nub, lo, hi, findex); + +// Future (v2) - planned +#include +auto solver = std::make_shared(); +auto result = solver->solve(A, b, x, options); +``` + +## Adding New Solvers + +To add a new solver: + +1. **Choose the category**: pivoting, projection, newton, or other +2. **Create header and source files** in the appropriate subdirectory +3. **Inherit from LCPSolver** base class +4. **Implement required virtual methods**: + - `solve(A, b, x, options)` + - `getName()` + - `getCategory()` +5. **Add to All.hpp** for convenience +6. **Write unit tests** in `tests/unit/math/` +7. **Document** algorithm, properties, and usage + +### Example Template + +```cpp +// pivoting/MyNewSolver.hpp +#pragma once + +#include "dart/math/lcp/Solver.hpp" + +namespace dart::math::pivoting { + +class MyNewSolver : public LCPSolver { +public: + MyNewSolver(); + ~MyNewSolver() override = default; + + LCPResult solve( + const Eigen::MatrixXd& A, + const Eigen::VectorXd& b, + Eigen::VectorXd& x, + const LCPOptions& options) override; + + std::string getName() const override { return "MyNewSolver"; } + std::string getCategory() const override { return "Pivoting"; } +}; + +} // namespace dart::math::pivoting +``` + +## Testing + +All solvers should have comprehensive tests: + +```cpp +#include +#include + +TEST(LemkeSolver, SimpleExample) { + auto solver = std::make_shared(); + + // Set up problem... + Eigen::MatrixXd A(2, 2); + Eigen::VectorXd b(2); + Eigen::VectorXd x(2); + + // Solve + auto result = solver->solve(A, b, x); + + // Verify + EXPECT_TRUE(result.succeeded()); + EXPECT_LT(result.complementarity, 1e-6); +} +``` + +## References + +See comprehensive documentation in `/docs/background/lcp/`: + +- `01_problem-statement.md` - LCP formulation and applications +- `02_overview.md` - Implementation status and overview +- `03_pivoting-methods.md` - Pivoting algorithms +- `04_projection-methods.md` - Projection/sweeping algorithms +- `05_newton-methods.md` - Newton-based algorithms +- `06_other-methods.md` - Interior point and specialized methods +- `07_selection-guide.md` - Practical selection guidelines + +## Backward Compatibility + +All legacy APIs remain fully functional: + +- `Lemke(M, q, z)` and `validate(M, z, q)` functions +- `SolveLCP()` template function +- `ODELCPSolver` class + +New code should prefer the modern API (v2) for consistency and future extensibility. diff --git a/docs/background/lcp/01_problem-statement.md b/docs/background/lcp/01_problem-statement.md new file mode 100644 index 0000000000000..b728207f2a041 --- /dev/null +++ b/docs/background/lcp/01_problem-statement.md @@ -0,0 +1,392 @@ +# Linear Complementarity Problem (LCP) + +**Navigation**: [Overview →](02_overview.md) + +## Definition + +The **Linear Complementarity Problem (LCP)** is defined as: + +``` +Given A ∈ ℝⁿˣⁿ and b ∈ ℝⁿ, find x ∈ ℝⁿ such that: + + w = Ax + b + w ≥ 0 + x ≥ 0 + xᵀw = 0 (complementarity condition) +``` + +The complementarity condition `xᵀw = 0` means that for each index `i`: + +``` +Either xᵢ = 0 OR wᵢ = 0 (or both) +``` + +This can also be written component-wise as: + +``` +For each i ∈ {1, ..., n}: + wᵢ = (Ax + b)ᵢ ≥ 0 + xᵢ ≥ 0 + xᵢ · wᵢ = 0 +``` + +## Equivalent Formulations + +### Minimum Map Reformulation + +``` +x = min(x, Ax + b) +``` + +where the minimum is taken component-wise. + +### Nonlinear Complementarity Problem (NCP) + +``` +F(x) = 0 +where F(x) = min(x, Ax + b) +``` + +### Variational Inequality (VI) + +``` +Find x ≥ 0 such that: + (Ax + b)ᵀ(y - x) ≥ 0 for all y ≥ 0 +``` + +### Quadratic Programming (QP) - When A is Symmetric PD + +``` +minimize: ½xᵀAx + xᵀb +subject to: x ≥ 0 +``` + +## Problem Variants + +### Standard LCP + +The basic form as defined above with unbounded variables constrained to be non-negative. + +### Boxed LCP (BLCP) + +LCP with box constraints on variables: + +``` +Find x such that: + l ≤ x ≤ u + w = Ax + b + For each i: + - If xᵢ = lᵢ, then wᵢ ≥ 0 + - If xᵢ = uᵢ, then wᵢ ≤ 0 + - If lᵢ < xᵢ < uᵢ, then wᵢ = 0 +``` + +Typical bounds: + +- Lower: `l ≤ 0` (often `-∞` or `-μN` for friction) +- Upper: `u ≥ 0` (often `+∞` or `+μN` for friction) + +### Mixed LCP (MLCP) + +Combination of equality constraints and complementarity: + +``` +Find x, z such that: + Ax + Bz + c = 0 (equality constraints) + w = Cx + Dz + e + w ≥ 0, z ≥ 0 + zᵀw = 0 (complementarity) +``` + +### LCP with Friction (FLCP) + +Special BLCP where bounds depend on other variables: + +``` +For friction at contact i: + -μNᵢ ≤ Fₜᵢ ≤ μNᵢ +where: + - Nᵢ is normal force + - Fₜᵢ is tangential friction force + - μ is friction coefficient +``` + +## Applications in Physics-Based Simulation + +### 1. Contact Mechanics + +**Unilateral Contact Constraints**: + +``` +For each contact point i: + dᵢ ≥ 0 (non-penetration) + Nᵢ ≥ 0 (no adhesion) + Nᵢ · dᵢ = 0 (complementarity: either separated or in contact) + +where: + - dᵢ = gap distance (signed distance function) + - Nᵢ = normal contact force +``` + +**LCP Formulation**: +After time discretization and linearization: + +``` +x = [N₁, N₂, ..., Nₙ]ᵀ (normal forces) +Ax + b represents velocity constraint violations +Complementarity ensures forces are zero when separated +``` + +### 2. Friction Modeling + +**Coulomb Friction as BLCP**: + +``` +For contact i with normal force Nᵢ: + Friction cone: ||Fₜᵢ|| ≤ μNᵢ + +Discretized (pyramid approximation): + -μNᵢ ≤ Fₜᵢˣ ≤ μNᵢ + -μNᵢ ≤ Fₜᵢʸ ≤ μNᵢ + +BLCP formulation: + lᵢ = -μNᵢ + uᵢ = +μNᵢ +``` + +### 3. Joint Limits + +**Joint Constraints**: + +``` +For revolute joint with limits θₘᵢₙ ≤ θ ≤ θₘₐₓ: + +BLCP formulation: + l = θₘᵢₙ + u = θₘₐₓ + Forces active only at limits +``` + +### 4. Rigid Body Dynamics + +**Time-Stepping Scheme**: + +``` +Velocity-level formulation: + M(v₊ - v₋) = h·fₑₓₜ + Jᵀλ + +where: + - M = mass matrix + - v₋, v₊ = velocities before/after contact + - h = time step + - J = contact Jacobian + - λ = contact impulses (LCP variable) + +LCP emerges from non-penetration and friction constraints +``` + +### 5. Fluid Simulation + +**Pressure Projection**: + +``` +Incompressible flow with boundaries: + ∇·u = 0 (divergence-free) + p ≥ 0 (pressure non-negative) + p·(u·n) = 0 (complementarity) + +Discretized → LCP for pressure field +``` + +## Mathematical Properties + +### Existence and Uniqueness + +**Theorem**: An LCP has a unique solution if A is: + +- **Strictly Copositive**: xᵀAx > 0 for all x ≠ 0, x ≥ 0 +- **P-matrix**: All principal minors are positive +- **Symmetric Positive Definite (PD)**: xᵀAx > 0 for all x ≠ 0 + +**Common Cases**: + +- Contact mechanics: A is often symmetric PSD (positive semi-definite) +- Joint constraints may make A singular (PSD but not PD) + +### Solvability Classes + +| Matrix Class | Solution Exists? | Solution Unique? | Solvable By | +| ------------- | ---------------- | ---------------- | ------------------- | +| Symmetric PD | Always | Yes | All methods | +| Symmetric PSD | Sometimes | Sometimes | Pivoting, iterative | +| P-matrix | Always | Yes | Pivoting | +| Copositive | Sometimes | Sometimes | Depends on b | +| General | Sometimes | Sometimes | Trial and error | + +### Degeneracy + +**Strict Complementarity**: For all i, either xᵢ > 0 OR wᵢ > 0 (but not both zero) + +**Degeneracy**: When xᵢ = wᵢ = 0 for some i + +- Makes active set identification difficult +- Can slow convergence of iterative methods +- Pivoting methods may cycle + +## Complexity + +### Computational Complexity + +- **General LCP**: NP-complete (worst case) +- **Special cases** (symmetric PD): Polynomial time + +### Practical Complexity + +For physics simulation with n contact points: + +- **Problem size**: n to 6n variables (normal + friction + bounds) +- **Matrix structure**: Often sparse (O(kn) non-zeros, k small) +- **Time discretization**: Solve LCP every time step + +## Standard Forms + +### Cottle-Dantzig Form + +``` +w = Mz + q +w ≥ 0, z ≥ 0 +wᵀz = 0 +``` + +Used in theoretical analysis and pivoting methods. + +### Physics Form + +``` +(Ax + b) ≥ 0 +x ≥ 0 +xᵀ(Ax + b) = 0 +``` + +Direct from physics constraints. + +### Optimization Form (A symmetric PD) + +``` +minimize: f(x) = ½xᵀAx + xᵀb +subject to: x ≥ 0 + +KKT conditions → LCP +``` + +## Relationship to Other Problems + +### Linear Programming (LP) + +``` +LP ⊂ LCP + +LP: minimize cᵀx subject to Ax ≤ b, x ≥ 0 +Can be reformulated as LCP +``` + +### Quadratic Programming (QP) + +``` +QP (with box constraints) ⊂ LCP + +When A is symmetric: + LCP ↔ QP with non-negativity +``` + +### Optimization + +``` +KKT conditions of constrained optimization +often lead to LCP/BLCP +``` + +### Game Theory + +``` +Nash equilibria in bimatrix games +can be found by solving LCP +``` + +## Why LCPs Matter for DART + +### Core Use Cases + +1. **Contact Resolution** + - Every contact point → LCP variables + - Friction cones → BLCP bounds + - Non-penetration → Complementarity + +2. **Constraint Satisfaction** + - Joint limits → BLCP + - Motor constraints → MLCP + - Closed kinematic chains → MLCP + +3. **Interactive Simulation** + - Real-time requires fast LCP solvers + - Trade-off: speed vs accuracy + - Iterative methods essential (O(n) per iteration) + +4. **High-Fidelity Simulation** + - Accurate contact forces need tight tolerances + - Newton methods or pivoting + - O(n³) acceptable for off-line + +### Solver Requirements + +| Requirement | Method Choice | +| ----------------------------- | ------------------------ | +| Real-time (30+ FPS) | PGS, PSOR, BGS | +| High accuracy | Newton, Pivoting | +| Large scenes (>1000 contacts) | NNCG, PGS | +| Ill-conditioned | Pivoting, Interior Point | +| Parallel hardware | Jacobi, Red-Black GS | + +## Key Challenges + +### Numerical Challenges + +1. **Ill-conditioning**: Large mass ratios, thin objects +2. **Degeneracy**: Multiple contacts at same point +3. **Sparsity**: Must exploit for large problems +4. **Warm-starting**: Critical for time-stepping + +### Modeling Challenges + +1. **Friction cone discretization**: Pyramid vs ellipse +2. **Time integration**: Implicit vs explicit +3. **Constraint stabilization**: Baumgarte, post-stabilization +4. **Regularization**: Trade-off with physical accuracy + +### Implementation Challenges + +1. **Matrix assembly**: Efficient Jacobian computation +2. **Solver selection**: Problem-dependent performance +3. **Parameter tuning**: Tolerances, iterations, relaxation +4. **Robustness**: Handling edge cases, degeneracies + +## Further Reading + +### Theory + +- **Cottle, Pang, Stone** (1992): "The Linear Complementarity Problem" - Comprehensive reference +- **Murty** (1988): "Linear Complementarity, Linear and Nonlinear Programming" - Theoretical foundations + +### Physics-Based Animation + +- **Erleben et al.** (2017): "Numerical Methods for Linear Complementarity Problems in Physics-Based Animation" - Direct application to simulation +- **Baraff** (1994): "Fast Contact Force Computation for Nonpenetrating Rigid Bodies" - Foundational paper + +### Optimization + +- **Nocedal & Wright** (1999): "Numerical Optimization" - QP and NCP connections +- **Ferris & Kanzow** (2002): "Engineering and Economic Applications of Complementarity Problems" - Applied perspective + +--- + +**Next**: [Overview of LCP Solvers →](02_overview.md) diff --git a/docs/background/lcp/02_overview.md b/docs/background/lcp/02_overview.md new file mode 100644 index 0000000000000..8ec2fbed7ac80 --- /dev/null +++ b/docs/background/lcp/02_overview.md @@ -0,0 +1,271 @@ +# LCP Solvers in DART + +**Navigation**: [← Problem Statement](01_problem-statement.md) | [Pivoting Methods →](03_pivoting-methods.md) + +## Implementation Status + +This section tracks which LCP solvers are currently implemented in DART (`dart/math/lcp/`). + +| Category | Method | Status | Location | Notes | +| ------------------ | -------------------------- | ------------------ | ----------------- | --------------------------- | +| **Pivoting** | Dantzig Principal Pivoting | ✅ Implemented | `dantzig/Lcp.hpp` | BLCP solver with friction | +| **Pivoting** | Lemke Complementary Pivot | ✅ Implemented | `Lemke.hpp` | Standard LCP solver | +| **Pivoting** | Baraff Incremental | ❌ Not implemented | - | Planned | +| **Projection** | PGS (Gauss-Seidel) | ❌ Not implemented | - | High priority for real-time | +| **Projection** | PSOR (Over-Relaxation) | ❌ Not implemented | - | Extension of PGS | +| **Projection** | Blocked Gauss-Seidel | ❌ Not implemented | - | For contact problems | +| **Projection** | NNCG (Conjugate Gradient) | ❌ Not implemented | - | Better convergence than PGS | +| **Projection** | Subspace Minimization | ❌ Not implemented | - | Hybrid PGS approach | +| **Newton** | Minimum Map Newton | ❌ Not implemented | - | High accuracy | +| **Newton** | Fischer-Burmeister Newton | ❌ Not implemented | - | Well-studied method | +| **Newton** | Penalized FB Newton | ❌ Not implemented | - | Extension of FB | +| **Interior Point** | Interior Point Method | ❌ Not implemented | - | Very robust | +| **Staggering** | Staggering Method | ❌ Not implemented | - | For coupled problems | + +**Legend**: ✅ Implemented | 🚧 In Progress | ❌ Not Implemented | 📋 Planned + +### Currently Implemented Solvers + +#### 1. Dantzig Principal Pivoting Method (`dantzig/Lcp.hpp`) + +- **Type**: Principal pivoting method for BLCP (Boxed Linear Complementarity Problem) +- **Algorithm**: Dantzig-Cottle principal pivoting +- **Source**: Derived from Open Dynamics Engine (ODE) +- **Named after**: George B. Dantzig (pioneer of linear programming and simplex method) +- **Features**: + - Supports bounded variables (lo, hi) + - Handles friction with `findex` parameter + - Unbounded variables support (`nub` parameter) + - Early termination option +- **Use Case**: General BLCP problems with bounds, friction constraints + +#### 2. Lemke Complementary Pivot Method (`Lemke.hpp`) + +- **Type**: Complementary pivoting method for standard LCP +- **Algorithm**: Lemke's algorithm with artificial variable +- **Named after**: Carlton E. Lemke (developed complementary pivot theory) +- **Features**: + - Standard LCP formulation: Mx = q + w, x >= 0, w >= 0, x^T w = 0 + - Includes solution validation function +- **Use Case**: Standard LCP problems without bounds + +### Utility Classes + +#### ODELCPSolver (`ODELCPSolver.hpp`) + +- **Type**: Wrapper/utility for contact mechanics problems +- **Implementation**: Uses Dantzig principal pivoting internally +- **Features**: + - Handles contact problems with friction + - Formulation transfer to/from ODE format + - Solution validation +- **Parameters**: + - `numContacts`: Number of contact points + - `mu`: Friction coefficient + - `numDir`: Number of friction directions +- **Use Case**: Convenience wrapper for contact mechanics with automatic problem formulation + +## Introduction + +Linear Complementarity Problems (LCPs) are fundamental to physics-based simulation, particularly in: + +- Contact mechanics +- Rigid body dynamics +- Fluid simulation +- Constraint solving +- Optimization problems + +### LCP Definition + +The standard LCP is defined as: + +``` +Find x such that: + Ax + b >= 0 + x >= 0 + x^T(Ax + b) = 0 +``` + +where: + +- `A` is an n×n matrix +- `b` is an n-dimensional vector +- `x` is the n-dimensional solution vector + +The complementarity condition `x^T(Ax + b) = 0` means that for each index i, either `x_i = 0` or `(Ax + b)_i = 0`. + +### Problem Variants + +#### BLCP (Boxed LCP) + +LCP with bounded variables: + +``` +Find x such that: + l <= x <= u + Ax + b complements x +``` + +where `l` and `u` are lower and upper bounds. + +#### MLCP (Mixed LCP) + +Combination of equality constraints and complementarity conditions. + +## Solver Categories Overview + +LCP solvers can be categorized into several main families: + +### 1. [Pivoting Methods](03_pivoting-methods.md) + +- **Direct** and **Incremental Pivoting** +- Exact solutions (if exists) +- Time: O(n^4), Storage: O(n^2) +- Best for: Small-medium problems, poorly conditioned matrices + +### 2. [Projection/Sweeping Methods](04_projection-methods.md) + +- **PGS**, **PSOR**, **Blocked Gauss-Seidel**, **NNCG** +- Iterative with linear convergence +- Time: O(n) per iteration, Storage: O(n) +- Best for: Real-time simulation, interactive applications + +### 3. [Newton Methods](05_newton-methods.md) + +- **Minimum Map**, **Fischer-Burmeister**, **Penalized FB** +- Superlinear to quadratic convergence +- Time: O(n^3) or O(n) per iteration +- Best for: High accuracy, off-line simulation + +### 4. [Other Methods](06_other-methods.md) + +- **Interior Point**, **Staggering**, **Specialized Methods** +- Problem-specific approaches +- Various convergence properties + +## Quick Selection Guide + +| Use Case | Recommended Method | Reason | +| -------------------- | ------------------------ | ----------------------- | +| Real-time simulation | PGS, PSOR, BGS | Fast O(n) iterations | +| High accuracy | Newton, Pivoting | Superlinear convergence | +| Large-scale | NNCG, PGS | Scalable, matrix-free | +| Poorly conditioned | Pivoting, Interior Point | Numerically robust | +| Contact mechanics | BGS, ODELCPSolver | Natural block structure | +| Parallel computing | Jacobi, Red-Black GS | Embarrassingly parallel | + +See [LCP Selection Guide](07_selection-guide.md) for detailed recommendations. + +## Documentation Structure + +- **[01_problem-statement.md](01_problem-statement.md)** - LCP definition, variants, and applications +- **[02_overview.md](02_overview.md)** (this file) - Implementation status and introduction +- **[03_pivoting-methods.md](03_pivoting-methods.md)** - Pivoting methods details +- **[04_projection-methods.md](04_projection-methods.md)** - Projection/sweeping methods details +- **[05_newton-methods.md](05_newton-methods.md)** - Newton methods details +- **[06_other-methods.md](06_other-methods.md)** - Interior Point, Staggering, and specialized methods +- **[07_selection-guide.md](07_selection-guide.md)** - Practical selection guidelines + +## Numerical Properties Comparison + +### Convergence Rates + +| Method Category | Convergence Rate | Iterations Needed | +| --------------- | ------------------------ | ----------------- | +| Pivoting | Exact (finite) | 1 (worst O(2^n)) | +| PGS/PSOR | Linear | 50-500 | +| Blocked Methods | Linear | 50-500 | +| NNCG | Linear to Superlinear | 20-200 | +| Interior Point | Superlinear | 10-50 | +| Newton | Superlinear to Quadratic | 5-20 | + +### Computational Cost per Iteration + +| Method | Time | Storage | Notes | +| -------------- | ------------------ | -------------- | ------------------------------ | +| Pivoting | O(n^3) | O(n^2) | With incremental factorization | +| PGS/PSOR | O(nk)\* | O(n) | k = max non-zeros per row | +| BGS | O(n·b^3) | O(n) | b = block size | +| NNCG | O(n) | O(n) | Same as PGS | +| Interior Point | O(n^3) or O(n)\*\* | O(n^2) or O(n) | Depends on solver | +| Newton | O(n^3) or O(n)\*\* | O(n^2) or O(n) | Depends on solver | + +\* k is typically small (< 10) for sparse problems +\*\* Direct solver vs iterative solver + +### Matrix Requirements + +| Method | Requirements | +| --------------- | --------------------------------------------------- | +| Dantzig/Lemke | None (general LCP) | +| Baraff Pivoting | Symmetric PD or PSD | +| PGS/PSOR | Non-zero diagonal (symmetric for convergence proof) | +| Newton Methods | None (but active set should be non-singular) | +| Interior Point | None | + +## Implementation Roadmap + +### Phase 1: Core Iterative Methods (High Priority) + +- [ ] Projected Gauss-Seidel (PGS) +- [ ] Projected SOR (PSOR) +- [ ] Basic termination criteria and merit functions + +### Phase 2: Blocked Methods (Medium Priority) + +- [ ] Blocked Gauss-Seidel (BGS) +- [ ] Per-contact block structure +- [ ] Direct 2D/3D sub-solvers + +### Phase 3: Advanced Iterative (Medium Priority) + +- [ ] Nonsmooth Nonlinear Conjugate Gradient (NNCG) +- [ ] Subspace Minimization (PGS-SM) +- [ ] Staggering methods + +### Phase 4: Newton Methods (Low Priority) + +- [ ] Minimum Map Newton +- [ ] Fischer-Burmeister Newton +- [ ] Projected line search +- [ ] Nonsmooth gradient descent (warm start) + +### Phase 5: Additional Methods (Future) + +- [ ] Interior Point method +- [ ] Baraff incremental pivoting +- [ ] Specialized methods (shock propagation, etc.) + +## References + +### Key Papers and Books + +1. **Cottle, R. W., Pang, J.-S., & Stone, R. E.** (1992). "The Linear Complementarity Problem" +2. **Baraff, D.** (1994). "Fast Contact Force Computation for Nonpenetrating Rigid Bodies" +3. **Erleben, K., et al.** (2017). "Numerical Methods for Linear Complementarity Problems in Physics-Based Animation" +4. **Fischer, A.** (1992). "A special Newton-type optimization method" +5. **Nocedal, J., & Wright, S.** (1999). "Numerical Optimization" + +### Software Implementations + +- **PATH**: Fischer-Burmeister with non-monotone line search +- **Num4LCP**: Research library with multiple solvers +- **Bullet Physics**: PGS and NNCG implementations +- **ODE**: Dantzig solver (basis for DART's implementation) +- **MOSEK, CPLEX**: Commercial QP solvers for symmetric PD matrices + +## Contributing + +When implementing new LCP solvers: + +1. Update the implementation status table in this document +2. Add comprehensive documentation to the appropriate detailed page +3. Include algorithm pseudocode +4. Document convergence properties and use cases +5. Add unit tests with known solutions +6. Benchmark against existing solvers +7. Update the selection guide with practical recommendations + +--- + +**Last Updated**: 2025-11-22 diff --git a/docs/background/lcp/03_pivoting-methods.md b/docs/background/lcp/03_pivoting-methods.md new file mode 100644 index 0000000000000..d136156f49881 --- /dev/null +++ b/docs/background/lcp/03_pivoting-methods.md @@ -0,0 +1,379 @@ +# Pivoting Methods for LCP + +**Navigation**: [← Overview](02_overview.md) | [Projection Methods →](04_projection-methods.md) + +## Overview + +Pivoting methods solve LCPs by exploiting their combinatorial nature through enumerating solution candidates. They can find exact solutions when they exist. + +## 1. Direct Methods for Small-Sized Problems + +### Description + +Geometric approaches for 2D and 3D LCPs using complementarity cones and angle intersection tests. + +### 2D Algorithm + +Given LCP with 2D vector `x`: + +``` +Test all 4 complementarity cones: + C₁ = [e₁, e₂] (both from identity) + C₂ = [a₁, e₂] (first from A, second from identity) + C₃ = [e₁, a₂] (first from identity, second from A) + C₄ = [a₁, a₂] (both from A) + +For each cone C: + if b is in positive cone of C: + return solution x = C⁻¹b +``` + +### 3D Algorithm + +Test all 8 complementarity cones using spherical triangle inclusion: + +``` +For cone C = [c₁, c₂, c₃]: + if det(C) > 0 and + (c₁ × c₂)·b >= 0 and + (c₂ × c₃)·b >= 0 and + (c₃ × c₁)·b >= 0: + return x = C⁻¹b +``` + +### Properties + +- **Time Complexity**: O(2ⁿ) for n-dimensional problem +- **Space Complexity**: O(n) +- **Convergence**: Exact solution (if exists) +- **Applicability**: Only 2D/3D problems + +### Use Cases + +- Building blocks for blocked splitting methods +- Sub-solvers in BGS for small contact blocks +- Testing and validation + +## 2. Dantzig Method ✅ (Implemented in DART) + +### Description + +BLCP solver using pivoting. Derived from ODE (Open Dynamics Engine). + +### Problem Formulation + +Given `(A, b, lo, hi)`, solve: + +``` +Ax = b + w, where each (xᵢ, wᵢ) satisfies one of: + (1) x = lo, w >= 0 (at lower bound) + (2) x = hi, w <= 0 (at upper bound) + (3) lo < x < hi, w = 0 (free variable) +``` + +### Key Features + +- **Bounded variables**: `lo[i] <= x[i] <= hi[i]` +- **Unbounded variables**: First `nub` variables have infinite bounds +- **Friction support**: `findex[i]` for friction cone constraints + - When `findex[i] >= 0`: bounds become `hi[i] = |hi[i] * x[findex[i]]|`, `lo[i] = -hi[i]` +- **Early termination**: Optional for faster approximate solutions + +### Algorithm Pseudocode + +```cpp +template +bool SolveLCP(int n, Scalar* A, Scalar* x, Scalar* b, Scalar* w, + int nub, Scalar* lo, Scalar* hi, int* findex) { + // Initialize basis and perform pivoting + // 1. Set up initial basis with unbounded variables + // 2. For each variable not in basis: + // a. Find entering variable + // b. Find leaving variable (ratio test) + // c. Perform pivot operation + // d. Update basis + // 3. Extract solution from final basis +} +``` + +### DART Implementation + +```cpp +#include + +using namespace dart::math; + +constexpr int n = 6; +double A[n*n], x[n], b[n], w[n], lo[n], hi[n]; +int findex[n]; + +// Initialize A, b, lo, hi, findex... + +bool success = SolveLCP( + n, A, x, b, w, + 0, // nub (unbounded variables) + lo, hi, + findex, + false // earlyTermination +); +``` + +### Properties + +- **Time Complexity**: O(n⁴) worst case +- **Space Complexity**: O(n²) +- **Convergence**: Exact solution +- **Matrix Requirements**: None (general BLCP) + +### Use Cases + +- General BLCP with bounds +- Contact problems with friction +- When exact solutions are needed + +## 3. Lemke Method ✅ (Implemented in DART) + +### Description + +Standard pivoting method for LCP using complementary pivoting. + +### Problem Formulation + +``` +Find z such that: + Mz = q + w + z >= 0, w >= 0 + z^T w = 0 +``` + +### Algorithm Overview + +``` +1. Initialize with artificial variable z₀ +2. Iterate: + a. Select entering variable (complementary to leaving) + b. Perform ratio test for leaving variable + c. Pivot to update basis + d. If z₀ leaves basis, solution found +3. Extract final solution +``` + +### DART Implementation + +```cpp +#include + +using namespace dart::math; + +Eigen::MatrixXd M(n, n); +Eigen::VectorXd q(n), z(n); + +// Initialize M and q... + +int result = Lemke(M, q, &z); + +// Validate solution +bool valid = validate(M, z, q); +``` + +### Properties + +- **Time Complexity**: O(n⁴) worst case +- **Space Complexity**: O(n²) +- **Convergence**: Exact solution (if exists) +- **Matrix Requirements**: None (general LCP) + +### Use Cases + +- Standard LCP problems +- When matrix A is not symmetric +- Validation and testing + +## 4. Baraff Incremental Pivoting ❌ (Not Implemented) + +### Description + +Incrementally builds active/free index sets while maintaining complementarity constraints as invariants. + +### Key Concepts + +- **Index Sets**: + - **Active set A**: indices where `xᵢ > 0` + - **Free set F**: indices where `yᵢ = (Ax + b)ᵢ > 0` + - **Unprocessed U**: indices not yet assigned + +### Algorithm Pseudocode + +``` +Initialize: A = ∅, F = ∅, U = {1,...,n} + +while U ≠ ∅: + Select j ∈ U + + # Try to make x_j positive + Find maximum x_j such that: + - All complementarity constraints satisfied + - x_j is limited by blocking constraints + + if blocking from A: + # Pivot: swap index from A to F + Continue inner pivoting loop + else if blocking from F: + # Pivot: swap index from F to A + Continue inner pivoting loop + else: + # No blocking, assign j to A or F + if x_j > 0: + A ← A ∪ {j} + else: + F ← F ∪ {j} + U ← U \ {j} +``` + +### Properties + +- **Time Complexity**: O(n⁴) with incremental factorization +- **Space Complexity**: O(n²) +- **Convergence**: Exact solution +- **Matrix Requirements**: Symmetric PD or PSD + +### Advantages + +- Can handle PSD matrices (common in contact problems) +- Incremental factorization reduces cost +- Maintains complementarity as invariant + +### Use Cases + +- Contact force problems +- When A is symmetric PSD +- Redundant contact constraints + +## Comparison Table + +| Method | Status | Problem Type | Time | Matrix Requirements | +| ------------ | --------------- | ------------ | ----- | ------------------- | +| Direct 2D/3D | Not implemented | LCP | O(2ⁿ) | None | +| Dantzig | ✅ Implemented | BLCP | O(n⁴) | None | +| Lemke | ✅ Implemented | LCP | O(n⁴) | None | +| Baraff | Not implemented | LCP | O(n⁴) | Symmetric PD/PSD | + +## When to Use Pivoting Methods + +### Advantages + +- ✅ Exact solutions (when they exist) +- ✅ Handle poorly conditioned matrices +- ✅ Work with large mass ratios +- ✅ Guaranteed finite termination + +### Disadvantages + +- ❌ High computational cost O(n⁴) +- ❌ Require full matrix assembly +- ❌ Not suitable for real-time with large n +- ❌ Memory intensive O(n²) + +### Recommended For + +- Small to medium problems (n < 100) +- Off-line simulations +- High accuracy requirements +- Poorly conditioned systems +- Validation of other solvers + +### Not Recommended For + +- Real-time interactive simulation +- Large-scale problems (n > 1000) +- When approximate solutions suffice +- Matrix-free implementations needed + +## Implementation Notes + +### For Dantzig (Already Implemented) + +- Arrays use C-style indexing +- Matrix A may be modified during solve +- Check return value for solution existence +- Use `findex` for friction cones carefully + +### For Lemke (Already Implemented) + +- Use Eigen matrices for convenience +- Always validate solution after solving +- Handle non-existent solutions gracefully + +### For Future Implementations + +- Consider incremental factorization (Baraff) +- Use sparse matrix representations when possible +- Implement efficient pivoting strategies +- Add comprehensive unit tests + +## References + +### Foundational Papers + +1. **Cottle, R. W., & Dantzig, G. B.** (1968). "Complementary pivot theory of mathematical programming". _Linear Algebra and its Applications_, 1(1), 103-125. + - Original complementary pivot algorithm + +2. **Lemke, C. E.** (1965). "Bimatrix equilibrium points and mathematical programming". _Management Science_, 11(7), 681-689. + - Lemke's complementary pivot algorithm with artificial variable + +3. **Lemke, C. E., & Howson Jr, J. T.** (1964). "Equilibrium points of bimatrix games". _Journal of the Society for Industrial and Applied Mathematics_, 12(2), 413-423. + - Original Lemke-Howson algorithm + +### Direct Methods + +4. **Murty, K. G.** (1988). _Linear complementarity, linear and nonlinear programming_. Heldermann Verlag. + - Comprehensive treatment of pivoting methods + - Chapter 3: Principal pivoting methods + +5. **Cottle, R. W., Pang, J. S., & Stone, R. E.** (1992). _The linear complementarity problem_. Academic press. + - Definitive reference on LCP theory + - Chapters 4-6: Pivoting algorithms and complexity + +### Baraff's Incremental Method + +6. **Baraff, D.** (1994). "Fast contact force computation for nonpenetrating rigid bodies". _Proceedings of the 21st annual conference on Computer graphics and interactive techniques_, 23-34. + - Incremental pivoting for contact mechanics + - Maintains complementarity as invariant + - O(n^4) with incremental factorization + +7. **Baraff, D.** (1989). "Analytical methods for dynamic simulation of non-penetrating rigid bodies". _ACM SIGGRAPH Computer Graphics_, 23(3), 223-232. + - Earlier work on contact force computation + - Foundation for incremental pivoting + +### ODE Implementation + +8. **Smith, R.** (2006). "Open Dynamics Engine" [Software]. http://www.ode.org/ + - Source of DART's Dantzig implementation + - BLCP solver with friction support + - License: BSD or LGPL + +9. **Smith, R.** (2000). "Open Dynamics Engine v0.035 User Guide". + - Documentation of ODE's LCP solver + - Details on `findex` friction cone implementation + +### Complexity and Theory + +10. **Chung, S. J.** (1989). "NP-completeness of the linear complementarity problem". _Journal of Optimization Theory and Applications_, 60(3), 393-399. + - Proves general LCP is NP-complete + +11. **Cottle, R. W., & Dantzig, G. B.** (1970). "A generalization of the linear complementarity problem". _Journal of Combinatorial Theory_, 8(1), 79-90. + - Generalized complementarity problems + +### Applications to Simulation + +12. **Stewart, D. E., & Trinkle, J. C.** (1996). "An implicit time-stepping scheme for rigid body dynamics with inelastic collisions and coulomb friction". _International Journal for Numerical Methods in Engineering_, 39(15), 2673-2691. + - Time-stepping formulation leading to LCP + - Contact mechanics application + +13. **Anitescu, M., & Potra, F. A.** (1997). "Formulating dynamic multi-rigid-body contact problems with friction as solvable linear complementarity problems". _Nonlinear Dynamics_, 14(3), 231-247. + - Velocity-level LCP formulation + - Theoretical foundation for pivoting in dynamics + +--- + +**Navigation**: [← Overview](02_overview.md) | [Projection Methods →](04_projection-methods.md) diff --git a/docs/background/lcp/04_projection-methods.md b/docs/background/lcp/04_projection-methods.md new file mode 100644 index 0000000000000..3d68790613d7d --- /dev/null +++ b/docs/background/lcp/04_projection-methods.md @@ -0,0 +1,559 @@ +# Projection/Sweeping Methods for LCP + +**Navigation**: [← Pivoting Methods](03_pivoting-methods.md) | [Newton Methods →](05_newton-methods.md) + +## Overview + +Projection methods (also called sweeping or splitting methods) are iterative solvers based on matrix splitting A = M - N. They are the most popular methods for real-time physics simulation due to their low O(n) per-iteration cost. + +## Core Concept: Matrix Splitting + +All projection methods use: + +``` +A = M - N (matrix splitting) +Mx^{k+1} = Nx^k + b (fixed-point iteration) +x^{k+1} = max(0, M^{-1}(Nx^k + b)) (projection onto positive orthant) +``` + +## 1. Jacobi Method ❌ (Not Implemented) + +### Splitting + +- M = D (diagonal of A) +- N = D - A + +### Update Rule + +``` +x_i^{k+1} = max(0, -r_i / A_{ii}) for all i in parallel +where r = b + Ax^k +``` + +### Properties + +- **Time**: O(n) per iteration +- **Storage**: O(n) +- **Convergence**: Linear (if converges) +- **Parallelization**: Fully parallel + +### Advantages/Disadvantages + +✅ Embarrassingly parallel - all updates independent +✅ Simple to implement +❌ Slower convergence than Gauss-Seidel +❌ May not converge for some problems + +## 2. Projected Gauss-Seidel (PGS) ❌ (Not Implemented, High Priority) + +### Splitting + +- M = D + L (diagonal + lower triangular) +- N = -U (negative upper triangular) + +### Update Rule + +``` +for i = 1 to n: + r_i = b_i + sum(A_{ij} * x_j, j=1..n) + x_i = max(0, -r_i / A_{ii}) +``` + +### Pseudocode + +``` +function PGS(A, b, x, max_iter, epsilon): + for iter = 1 to max_iter: + x_old = x + for i = 1 to n: + r_i = b_i + dot(A[i,:], x) + x[i] = max(0, -r_i / A[i,i]) + + if ||x - x_old|| < epsilon: + break + return x +``` + +### Properties + +- **Time**: O(nk) per iteration (k = max non-zeros per row) +- **Storage**: O(n) +- **Convergence**: Linear for symmetric PSD +- **Parallelization**: Sequential + +### Advantages/Disadvantages + +✅ Fast O(n) iterations +✅ Low memory footprint +✅ Works well for contact problems +✅ In-place updates +❌ Sequential (not parallel) +❌ Convergence depends on sweep order +❌ May be slow for ill-conditioned problems + +### Use Cases + +- Real-time rigid body simulation +- Contact force computation +- Interactive applications +- First-choice for most physics engines + +## 3. Projected SOR (PSOR) ❌ (Not Implemented, High Priority) + +### Splitting + +- M = (D + λL)/λ +- N = ((1-λ)D - λU)/λ + +### Update Rule + +``` +for i = 1 to n: + r_i = b_i + sum(A_{ij} * x_j, j=1..n) + x_i = max(0, x_i - lambda * r_i / A_{ii}) +``` + +### Relaxation Parameter λ + +- **λ = 1**: Reduces to PGS +- **0 < λ < 1**: Under-relaxation (more stable) +- **1 < λ < 2**: Over-relaxation (faster convergence) +- **Typical**: λ = 1.2 to 1.5 + +### Properties + +- **Time**: O(nk) per iteration +- **Storage**: O(n) +- **Convergence**: Can be faster than PGS with good λ +- **Parallelization**: Sequential + +### Advantages/Disadvantages + +✅ Faster convergence than PGS with good λ +✅ Same computational cost as PGS +❌ Requires tuning λ parameter +❌ Bad λ can make convergence worse + +## 4. Symmetric PSOR ❌ (Not Implemented) + +### Algorithm + +Forward sweep (i = 1 to n) followed by backward sweep (i = n to 1). + +### Properties + +- Reduces sweep-order dependency +- 2× cost per iteration +- Better convergence behavior + +## 5. Blocked Gauss-Seidel (BGS) ❌ (Not Implemented, Medium Priority) + +### Description + +Applies Gauss-Seidel to blocks of variables rather than individual variables. Also known as Nonsmooth Contact Dynamics (NSCD). + +### Block Structure for Contact Problems + +``` +Block i contains variables for contact point i: + - Normal impulse x_n,i + - Tangential impulses x_t1,i, x_t2,i (or 4 for pyramid) + - Slack variable β_i (for friction cone) +``` + +### Algorithm + +``` +for iter = 1 to max_iter: + for each block i: + # Compute residual for block + r_i = b_i + sum(A_{ij} * x_j, j=1..num_blocks) + + # Solve sub-LCP for block i + x_i = SolveSubLCP(A_{ii}, r_i, bounds_i) +``` + +### Sub-LCP Solvers + +- **1D normal**: Direct solve +- **2D/3D friction**: Direct geometric method +- **4D pyramid**: Direct or small iterative +- **General**: Any LCP solver + +### Properties + +- **Time**: O(n·b³) per iteration (b = block size) +- **Storage**: O(n) +- **Convergence**: Linear +- **Block size**: Typically 1-6 variables + +### Advantages/Disadvantages + +✅ Natural for contact problems +✅ Can use exact sub-solvers +✅ Better convergence than scalar PGS +✅ Flexible block definitions +❌ More complex than PGS +❌ Higher cost per iteration + +### Use Cases + +- Contact force problems +- One block per contact point +- When sub-problems are small and cheap + +## 6. Nonsmooth Nonlinear Conjugate Gradient (NNCG) ❌ (Not Implemented) + +### Description + +Conjugate gradient acceleration of PGS using Fletcher-Reeves formula. + +### Algorithm + +``` +function NNCG(A, b, x, max_iter): + r = PGS_iteration(x) - x # residual + p = r # search direction + + for iter = 1 to max_iter: + x = x + p # full step + r_new = PGS_iteration(x) - x + + beta = ||r_new||² / ||r||² + + if beta > 1: + p = r_new # restart + else: + p = r_new + beta * p + + r = r_new +``` + +### Properties + +- **Time**: O(n) per iteration (same as PGS) +- **Storage**: O(n) (need to store p vector) +- **Convergence**: Linear to superlinear (empirically) +- **Restart**: Every 10-20 iterations + +### Advantages/Disadvantages + +✅ Better convergence than PGS +✅ Same per-iteration cost as PGS +✅ Handles large mass ratios better +❌ Slightly more complex +❌ Needs restart strategy + +### Use Cases + +- Large-scale problems +- Better accuracy than PGS +- When PGS converges too slowly + +## 7. Subspace Minimization (PGS-SM) ❌ (Not Implemented) + +### Description + +Two-phase hybrid: PGS for active set estimation + direct solve for refinement. + +### Algorithm + +``` +while not converged: + # Phase 1: PGS for active set estimation + for k_pgs iterations: + PGS_iteration(x) + + # Phase 2: Subspace minimization + for k_sm iterations: + # Partition into L (lower), U (upper), A (active) + L = {i | x_i = l_i} + U = {i | x_i = u_i} + A = {i | l_i < x_i < u_i} + + # Solve reduced system + A_{AA} * x_A = -(b_A + A_{AL}*l + A_{AU}*u) + + # Project back + x_A = min(u_A, max(l_A, x_A)) +``` + +### Properties + +- **Time**: O(n) for PGS + O(|A|³) for subspace +- **Storage**: O(n) +- **Convergence**: Better than pure PGS + +### Use Cases + +- Small to medium problems with joints +- When PGS alone is not accurate enough +- Problems with clear active/inactive distinction + +## 8. Red-Black Gauss-Seidel ❌ (Not Implemented) + +### Description + +Two-color blocking for parallelization. + +### Algorithm + +``` +# Color variables: red or black (checkerboard pattern) +for iter = 1 to max_iter: + # Phase 1: Update all red variables in parallel + parallel for i in red_indices: + update x_i + + # Phase 2: Update all black variables in parallel + parallel for i in black_indices: + update x_i +``` + +### Properties + +- **Parallelization**: 2-phase parallel +- **Convergence**: Between Jacobi and Gauss-Seidel + +### Use Cases + +- GPU implementations +- Parallel computing +- Domain decomposition + +## Termination Criteria + +### Absolute Convergence + +``` +||Ax + b|| < epsilon_abs +``` + +Typical: epsilon_abs = 1e-6 + +### Relative Convergence + +``` +||x^{k+1} - x^k|| / ||x^k|| < epsilon_rel +``` + +Typical: epsilon_rel = 1e-4 + +### Complementarity + +``` +||x ⊙ (Ax + b)|| < epsilon_comp +``` + +where ⊙ is element-wise product + +### Maximum Iterations + +``` +iter >= max_iter +``` + +Typical: 50-100 for real-time, 1000+ for accuracy + +## Merit Functions + +### QP Formulation + +``` +phi(x) = 0.5 * x^T * A * x + x^T * b +``` + +### Modified for x=0 Safety + +``` +phi(x) = ||x ⊙ (Ax + b)|| +``` + +### Infinity Norm (Cheap to Compute) + +``` +phi(x) = max_i |x_i| +``` + +## Comparison Table + +| Method | Status | Parallel | Convergence | Best For | +| --------- | ------------- | -------- | ----------- | --------------------- | +| Jacobi | ❌ | Yes | Slow | Parallel hardware | +| PGS | ❌ (Priority) | No | Linear | Real-time | +| PSOR | ❌ (Priority) | No | Linear | Real-time with tuning | +| BGS | ❌ | No | Linear | Contact problems | +| NNCG | ❌ | No | Superlinear | Large-scale | +| PGS-SM | ❌ | No | Better | Medium problems | +| Red-Black | ❌ | 2-phase | Medium | GPU | + +## Implementation Priority + +### Phase 1 (Essential for Real-Time) + +1. **PGS** - Core method, highest priority +2. **PSOR** - Extension of PGS with relaxation +3. **Termination criteria** - Multiple stopping conditions +4. **Merit functions** - For convergence monitoring + +### Phase 2 (For Contact Problems) + +5. **BGS** - Natural for multi-contact scenarios +6. **Direct 2D/3D sub-solvers** - For BGS blocks + +### Phase 3 (Advanced) + +7. **NNCG** - Better convergence for large systems +8. **PGS-SM** - Hybrid approach + +## When to Use Projection Methods + +### Advantages + +✅ O(n) per-iteration cost +✅ Low memory O(n) +✅ Simple to implement +✅ Matrix-free possible +✅ Predictable performance + +### Disadvantages + +❌ Linear convergence (slow) +❌ Many iterations needed (50-500) +❌ Convergence depends on conditioning +❌ May not converge for all matrices + +### Recommended For + +- Real-time interactive simulation +- Contact force problems +- Large-scale systems +- When approximate solutions suffice +- First choice for physics engines + +### Not Recommended For + +- High accuracy requirements (use Newton instead) +- Very ill-conditioned systems (use pivoting instead) +- When exact solutions needed (use pivoting instead) + +## References + +### Classical Iterative Methods + +1. **Young, D. M.** (1971). _Iterative solution of large linear systems_. Academic press. + - Classical Gauss-Seidel and SOR methods + - Chapters 3-4: Convergence theory for splitting methods + +2. **Saad, Y.** (2003). _Iterative methods for sparse linear systems_ (2nd ed.). SIAM. + - Modern treatment of iterative methods + - Chapter 4: Relaxation methods (Jacobi, GS, SOR) + +### Projected Gauss-Seidel for LCP + +3. **Murty, K. G.** (1988). _Linear complementarity, linear and nonlinear programming_. Heldermann Verlag. + - Chapter 10: Iterative methods for LCP + - Convergence analysis for symmetric PSD matrices + +4. **Mangasarian, O. L.** (1977). "Solution of symmetric linear complementarity problems by iterative methods". _Journal of Optimization Theory and Applications_, 22(4), 465-485. + - Convergence theory for PGS on symmetric matrices + - Sufficient conditions for convergence + +5. **Cryer, C. W.** (1971). "The solution of a quadratic programming problem using systematic overrelaxation". _SIAM Journal on Control_, 9(3), 385-392. + - PSOR method for QP (equivalent to LCP when A symmetric) + - Optimal relaxation parameter analysis + +### Blocked Gauss-Seidel (NSCD) + +6. **Moreau, J. J.** (1988). "Unilateral contact and dry friction in finite freedom dynamics". _Nonsmooth mechanics and applications_, 1-82. Springer. + - Nonsmooth contact dynamics formulation + - Theoretical foundation for BGS in contact mechanics + +7. **Jean, M.** (1999). "The non-smooth contact dynamics method". _Computer methods in applied mechanics and engineering_, 177(3-4), 235-257. + - NSCD method = Blocked Gauss-Seidel for contacts + - Per-contact block structure + - Applications to granular materials + +8. **Acary, V., & Brogliato, B.** (2008). _Numerical methods for nonsmooth dynamical systems: applications in mechanics and electronics_. Springer Science & Business Media. + - Comprehensive treatment of NSCD/BGS + - Chapter 7: Time-stepping schemes + - Chapter 8: Numerical methods for LCP/MLCP + +9. **Anitescu, M., & Tasora, A.** (2010). "An iterative approach for cone complementarity problems for nonsmooth dynamics". _Computational Optimization and Applications_, 47(2), 207-235. + - Convergence analysis of Gauss-Seidel for friction cones + - Practical implementation considerations + +### Nonsmooth Nonlinear Conjugate Gradient (NNCG) + +10. **Silcowitz, M., Niebe, S., & Erleben, K.** (2009). "Nonsmooth nonlinear conjugate gradient method for interactive contact force problems". _The Visual Computer_, 25(5), 893-905. + - NNCG method for contact problems + - Fletcher-Reeves formula adaptation + - Better convergence than PGS for large mass ratios + +11. **Silcowitz, M., Niebe, S., & Erleben, K.** (2010). "A nonsmooth nonlinear conjugate gradient method for interactive contact force problems". _The Visual Computer_, 26(6-8), 893-901. + - Extended version with more implementation details + - Performance comparison with PGS and other methods + +12. **Fletcher, R., & Reeves, C. M.** (1964). "Function minimization by conjugate gradients". _The Computer Journal_, 7(2), 149-154. + - Original Fletcher-Reeves conjugate gradient + - Foundation for NNCG adaptation + +### Subspace Minimization + +13. **Silcowitz-Hansen, M., Erleben, K., & Niebe, S.** (2010). "A nonsmooth Newton method with applications to computer animation". In _MATHMOD 2009-6th Vienna International Conference on Mathematical Modelling_. + - Subspace minimization approach + - Combines PGS with direct solvers + - Active set prediction + +14. **Kaufman, D. M., Sueda, S., James, D. L., & Pai, D. K.** (2008). "Staggered projections for frictional contact in multibody systems". _ACM Transactions on Graphics (TOG)_, 27(5), 1-11. + - Related subspace approach + - Staggering for normal/tangential forces + +### Red-Black and Parallel Methods + +15. **Adams, L. M.** (1982). "Iterative algorithms for large sparse linear systems on parallel computers". _NASA Technical Memorandum_, 83267. + - Red-black ordering for parallelization + - Applications to domain decomposition + +16. **Hasenbusch, M., Lana, A., & Marcu, M.** (1999). "Accelerated Monte Carlo for fermions". _Nuclear Physics B-Proceedings Supplements_, 73(1-3), 864-866. + - Red-black ordering applications + - Checkerboard pattern for parallel updates + +### Convergence Theory + +17. **Luo, Z. Q., & Tseng, P.** (1992). "On the convergence of the coordinate descent method for convex differentiable minimization". _Journal of Optimization Theory and Applications_, 72(1), 7-35. + - General convergence theory + - Applicable to PGS as coordinate descent + +18. **Bertsekas, D. P.** (1999). _Nonlinear programming_ (2nd ed.). Athena scientific. + - Chapter 2.7: Coordinate descent methods + - Convergence for convex problems + +### Physics Simulation Applications + +19. **Catto, E.** (2005). "Iterative dynamics with temporal coherence". In _Game Developer Conference_. + - PGS in Box2D physics engine + - Practical implementation and warm-starting + +20. **Coumans, E.** (2015). "Bullet physics simulation". In _ACM SIGGRAPH 2015 Courses_, 1-87. + - PGS and NNCG in Bullet Physics + - Performance comparisons and best practices + +21. **Tonge, R., Benevolenski, F., & Voroshilov, A.** (2012). "Mass splitting for jitter-free parallel rigid body simulation". _ACM Transactions on Graphics (TOG)_, 31(4), 1-8. + - Jacobi-style parallel contact resolution + - Domain decomposition strategies + +### Implementation and Performance + +22. **Erleben, K.** (2013). "Numerical methods for linear complementarity problems in physics-based animation". In _ACM SIGGRAPH 2013 Courses_, 8. + - Comprehensive survey of PGS, PSOR, NNCG, etc. + - Implementation details and performance analysis + +23. **Erleben, K., Silcowitz-Hansen, M., & Niebe, S.** (2017). "Numerical methods for linear complementarity problems in physics-based animation". _Synthesis Lectures on Computer Graphics and Animation_, 11(2), 1-159. + - Extended book version of the survey + - Definitive reference for projection methods + - Detailed pseudocode and analysis + +--- + +**Navigation**: [← Pivoting Methods](03_pivoting-methods.md) | [Newton Methods →](05_newton-methods.md) diff --git a/docs/background/lcp/05_newton-methods.md b/docs/background/lcp/05_newton-methods.md new file mode 100644 index 0000000000000..e601fef8472b0 --- /dev/null +++ b/docs/background/lcp/05_newton-methods.md @@ -0,0 +1,559 @@ +# Newton Methods for LCP + +**Navigation**: [← Projection Methods](04_projection-methods.md) | [Other Methods →](06_other-methods.md) + +## Overview + +Newton methods solve LCPs by reformulating them as nonsmooth root-finding problems and applying generalized Newton iteration. They offer superlinear to quadratic convergence but require globalization for robustness. + +**All Newton methods are currently not implemented** but documented here for future reference. + +## Common Framework + +All Newton methods follow this pattern: + +``` +1. Reformulate LCP as H(x) = 0 (nonsmooth equation) +2. Compute generalized Newton direction: J*Δx = -H(x) +3. Line search for step length t +4. Update: x^{k+1} = max(0, x^k + t*Δx^k) +5. Repeat until convergence +``` + +## 1. Minimum Map Newton Method ❌ (Not Implemented) + +### Reformulation + +``` +H(x) = min(x, Ax + b) = 0 +``` + +### B-Derivative (Generalized Jacobian) + +``` +Partition indices into: + A = {i | y_i = (Ax+b)_i < x_i} (active set) + F = {i | y_i = (Ax+b)_i >= x_i} (free set) + +Then: + JH = [ A_AA ] (block for active set) + [ I_FF ] (identity for free set) +``` + +### Newton Equation + +``` +Reduced system (only for active set): + A_AA * Δx_A = -H_A + +For free set: + Δx_F = -H_F = -x_F (directly set to zero) +``` + +### Algorithm Pseudocode + +``` +while not converged: + # Compute residual + y = Ax + b + H = min(x, y) + + # Partition into active/free sets + A = {i | y_i < x_i} + F = {i | y_i >= x_i} + + # Solve reduced Newton equation + Δx_A = solve(A_AA, -H_A) + Δx_F = -x_F + + # Line search + t = line_search(x, Δx, H) + + # Update with projection + x = max(0, x + t*Δx) +``` + +### Properties + +- **Time**: O(n³) with direct solver, O(n) with iterative +- **Storage**: O(n²) with direct solver, O(n) with iterative +- **Convergence**: Superlinear to quadratic +- **Matrix Requirements**: A_AA should be non-singular + +### Advantages/Disadvantages + +✅ Fast convergence (5-20 iterations) +✅ High accuracy achievable +✅ Well-studied mathematically +❌ Sensitive to starting point +❌ Needs globalization (line search) +❌ More complex than projection methods + +## 2. Fischer-Burmeister Newton Method ❌ (Not Implemented) + +### Reformulation + +``` +For each component: + phi_FB(x_i, y_i) = sqrt(x_i² + y_i²) - x_i - y_i = 0 +where y_i = (Ax + b)_i +``` + +### Generalized Jacobian + +``` +For (x_i, y_i) != (0, 0): + p_i = x_i / sqrt(x_i² + y_i²) - 1 + q_i = y_i / sqrt(x_i² + y_i²) - 1 + +JF = diag(p) + diag(q)*A + +For (x_i, y_i) = (0, 0): + Can choose any (a_i, b_i) with ||(a_i, b_i)|| <= 1 +``` + +### Strategies for Singularity + +1. **Random**: Pick random (a, b) in unit circle +2. **Perturbation**: Use epsilon instead of exact 0 +3. **Finite Difference**: Approximate with (F(x+h\*p) - F(x))/h +4. **Analytic**: Use (a, b) = (0, 0) or (-1/sqrt(2), -1/sqrt(2)) + +### Algorithm Pseudocode + +``` +while not converged: + # Compute Fischer-Burmeister function + y = Ax + b + for i = 1 to n: + F_i = sqrt(x_i² + y_i²) - x_i - y_i + + # Build generalized Jacobian + JF = compute_jacobian(x, y) + + # Solve Newton equation (with iterative solver) + Δx = solve(JF, -F) # Use GMRES or PCG + + # Line search + t = line_search(x, Δx, F) + + # Update with projection + x = max(0, x + t*Δx) +``` + +### Properties + +- **Time**: O(n³) or O(n) per iteration +- **Storage**: O(n²) or O(n) +- **Convergence**: Superlinear to quadratic +- **Smoothness**: Nonsmooth only at (0,0) + +### Advantages/Disadvantages + +✅ Smoother than minimum map (only singular at origin) +✅ Well-studied in optimization literature +✅ PATH solver uses this reformulation +❌ Singularity handling needed +❌ More complex than minimum map + +## 3. Penalized Fischer-Burmeister Newton ❌ (Not Implemented) + +### Reformulation + +``` +phi_lambda(x, y) = lambda * phi_FB(x, y) - (1-lambda) * max(x,0) * max(y,0) +``` + +where 0 < lambda <= 1 + +### Effect of lambda Parameter + +- **lambda = 1**: Standard Fischer-Burmeister +- **lambda < 1**: More penalty on complementarity +- **lambda ~ 0.5**: Good balance (typical) + +### Generalized Jacobian + +``` +Combines Fischer-Burmeister Jacobian with penalty term: + +For x_i > 0, y_i > 0: + p_i = lambda * (x_i/||z_i|| - 1) - (1-lambda) * y_i + q_i = lambda * (y_i/||z_i|| - 1) - (1-lambda) * x_i + where z_i = sqrt(x_i² + y_i²) + +For x_i = 0 or y_i = 0 (but not both zero): + Special formulas apply + +For x_i = y_i = 0: + Use choices similar to standard FB +``` + +### Properties + +- **Time**: O(n³) or O(n) per iteration +- **Storage**: O(n²) or O(n) +- **Convergence**: Similar to Fischer-Burmeister +- **Tuning**: lambda parameter affects convergence + +### Advantages/Disadvantages + +✅ Additional tuning parameter +✅ Can improve convergence for some problems +❌ More complex Jacobian +❌ Requires tuning lambda + +## Supporting Methods + +### Line Search (Projected Armijo Back-Tracking) + +Essential for globalizing Newton methods. + +``` +function line_search(x, Δx, H): + # Parameters + alpha = 1e-4 # Sufficient decrease + beta = 0.5 # Step reduction + t = 1.0 # Initial step + + # Current merit function + phi_0 = 0.5 * ||H(x)||² + phi_prime_0 = H(x)^T * JH * Δx # Directional derivative + + # Back-tracking + while t > epsilon: + x_new = max(0, x + t*Δx) + phi_t = 0.5 * ||H(x_new)||² + + # Armijo condition + if phi_t <= phi_0 + alpha * t * phi_prime_0: + return t + + t = beta * t + + return t +``` + +### Nonsmooth Gradient Descent (Warm Start) + +Used to compute good starting iterate for Newton methods. + +``` +function gradient_descent(x, max_iter): + for iter = 1 to max_iter: + # Compute gradient of merit function + H = reformulation(x) + grad = JH^T * H + + # Simple line search + t = line_search(x, -grad, H) + + # Update + x = max(0, x - t * grad) + + # Early termination if good enough + if ||H|| < threshold: + break + + return x +``` + +### Subsystem Solvers + +Newton equation can be solved with iterative methods: + +**For Symmetric Systems (Minimum Map):** + +``` +Use PCG (Preconditioned Conjugate Gradient): + - Requires A_AA to be symmetric PD + - Incomplete Cholesky preconditioner + - Tolerance: ||r|| < gamma * ||H|| +``` + +**For General Systems (Fischer-Burmeister):** + +``` +Use GMRES (Generalized Minimal Residual): + - Works for any non-singular matrix + - Higher storage than PCG + - Tolerance: ||r|| < gamma * ||H|| +``` + +### Merit Functions + +All Newton methods use natural merit function: + +``` +phi(x) = 0.5 * ||H(x)||² +``` + +Properties: + +- phi(x) >= 0 +- phi(x) = 0 iff x is solution +- Descent direction: ∇phi = JH^T \* H + +## Comparison Table + +| Method | Smoothness | Jacobian Complexity | Best For | +| ------------------ | -------------------- | ------------------- | ------------ | +| Minimum Map | Nonsmooth everywhere | Simple (blocks) | General use | +| Fischer-Burmeister | Smooth except origin | Medium | Well-studied | +| Penalized FB | Less smooth | Complex | Tunable | + +## Implementation Strategy + +### Phase 1: Core Newton Framework + +1. Merit function computation +2. Projected line search +3. Termination criteria + +### Phase 2: Minimum Map Newton + +4. Active/free set partitioning +5. Reduced Newton equation solver +6. GMRES subsystem solver + +### Phase 3: Supporting Methods + +7. Nonsmooth gradient descent +8. Warm start from PGS + +### Phase 4: Fischer-Burmeister (Optional) + +9. FB reformulation +10. Generalized Jacobian +11. Singularity handling + +## When to Use Newton Methods + +### Advantages + +✅ Fast convergence (5-20 iterations) +✅ High accuracy (1e-8 to 1e-12) +✅ Superlinear/quadratic convergence rate +✅ Matrix-free with iterative solvers + +### Disadvantages + +❌ Complex implementation +❌ Sensitive to starting point +❌ Requires line search for robustness +❌ Not guaranteed to converge from arbitrary start + +### Recommended For + +- Off-line simulations +- High accuracy requirements +- When computational budget allows +- Validation and benchmarking + +### Not Recommended For + +- Real-time interactive simulation (use PGS instead) +- When robustness is critical (use pivoting instead) +- Very large problems without good warm start + +## Best Practices + +### Starting Iterates + +- Use x = 0 as default +- Warm start with PGS (5-10 iterations) +- Warm start with gradient descent +- Use previous time-step solution + +### Convergence Monitoring + +- Absolute: ||H|| < 1e-8 +- Relative: ||H^{k+1}|| / ||H^k|| < 1e-6 +- Maximum iterations: 20-50 +- Detect stagnation and divergence + +### Subsystem Solver Tolerance + +- Don't need exact Newton direction +- Use ||r|| < 0.1 \* ||H|| for descent +- Tighten tolerance as H → 0 + +### Fallback Strategy + +When Newton fails: + +1. Try gradient descent for few iterations +2. Restart Newton +3. Fall back to PGS +4. Use pivoting method + +## References + +### Nonsmooth Analysis and Generalized Derivatives + +1. **Clarke, F. H.** (1990). _Optimization and nonsmooth analysis_. SIAM. + - Chapter 2: Generalized gradients and subdifferentials + - Chapter 6: B-differentiability + - Theoretical foundation for nonsmooth Newton methods + +2. **Qi, L., & Sun, J.** (1993). "A nonsmooth version of Newton's method". _Mathematical programming_, 58(1), 353-367. + - Generalized Newton method for nonsmooth equations + - B-differentiability and semismooth functions + - Superlinear convergence theory + +3. **Pang, J. S., & Qi, L.** (1993). "Nonsmooth equations: motivation and algorithms". _SIAM Journal on Optimization_, 3(3), 443-465. + - Reformulation of complementarity as nonsmooth equations + - Newton methods for NCP + - Global convergence with line search + +### Minimum Map Newton Method + +4. **Fischer, A.** (1992). "A special Newton-type optimization method". _Optimization_, 24(3-4), 269-284. + - Minimum map reformulation H(x) = min(x, F(x)) + - B-derivative computation + - Superlinear convergence + +5. **Sun, D., & Qi, L.** (1999). "Solving variational inequality problems via smoothing-nonsmooth reformulations". _Journal of Computational and Applied Mathematics_, 129(1-2), 37-62. + - Minimum map approach for VI and LCP + - Active set identification + - Reduced Newton system + +6. **Facchinei, F., & Pang, J. S.** (2003). _Finite-dimensional variational inequalities and complementarity problems_. Springer Science & Business Media. + - Comprehensive treatment in Chapters 7-8 + - Minimum map and Fischer-Burmeister methods + - Theory and algorithms + +### Fischer-Burmeister Newton Method + +7. **Fischer, A.** (1992). "A special newton-type optimization method". _Optimization_, 24(3-4), 269-284. + - Fischer-Burmeister function φ_FB(a,b) = √(a²+b²) - a - b + - Nonsmooth Newton method + - Superlinear convergence + +8. **Burmeister, W.** (1990). "Monotone Komplementaritätsprobleme und Variationsungleichungen". PhD thesis, University of Hamburg. + - Original Fischer-Burmeister function + - German dissertation + +9. **De Luca, T., Facchinei, F., & Kanzow, C.** (1996). "A semismooth equation approach to the solution of nonlinear complementarity problems". _Mathematical programming_, 75(3), 407-439. + - Semismooth analysis of Fischer-Burmeister + - Generalized Jacobian computation + - Handling singularity at origin + +10. **Kanzow, C.** (1996). "Some noninterior continuation methods for linear complementarity problems". _SIAM Journal on Matrix Analysis and Applications_, 17(4), 851-868. + - Fischer-Burmeister path following + - Regularization strategies + - Global convergence + +### Penalized Fischer-Burmeister + +11. **Chen, B., & Harker, P. T.** (1997). "Smooth approximations to nonlinear complementarity problems". _SIAM Journal on Optimization_, 7(2), 403-420. + - Penalized versions of NCP functions + - Smoothing parameter effects + - Convergence analysis + +12. **Chen, C., & Mangasarian, O. L.** (1996). "A class of smoothing functions for nonlinear and mixed complementarity problems". _Computational Optimization and Applications_, 5(2), 97-138. + - General smoothing function framework + - Penalized Fischer-Burmeister as special case + - Numerical results + +### PATH Solver (Fischer-Burmeister Implementation) + +13. **Ferris, M. C., & Munson, T. S.** (2000). "Complementarity problems in GAMS and the PATH solver". _Journal of Economic Dynamics and Control_, 24(2), 165-188. + - PATH solver implementation + - Fischer-Burmeister with non-monotone line search + - GAMS/Matlab/Python interfaces + +14. **Dirkse, S. P., & Ferris, M. C.** (1995). "The PATH solver: a non-monotone stabilization scheme for mixed complementarity problems". _Optimization methods and software_, 5(2), 123-156. + - Non-monotone line search + - Crash techniques for initial point + - Robust implementation details + +15. **Ferris, M. C., & Kanzow, C.** (2002). "Engineering and economic applications of complementarity problems". _SIAM review_, 44(1), 1-37. + - Applications survey + - PATH solver case studies + - Comparison with other methods + +### Line Search and Globalization + +16. **Armijo, L.** (1966). "Minimization of functions having Lipschitz continuous first partial derivatives". _Pacific Journal of mathematics_, 16(1), 1-3. + - Original Armijo condition for sufficient decrease + - Foundation for backtracking line search + +17. **Nocedal, J., & Wright, S. J.** (1999). _Numerical optimization_. Springer. + - Chapter 3: Line search methods + - Chapter 11: Nonlinear least squares (relevant for merit functions) + - Practical algorithms + +18. **Dennis, J. E., & Schnabel, R. B.** (1996). _Numerical methods for unconstrained optimization and nonlinear equations_. SIAM. + - Chapter 6: Line search + - Chapter 8: Trust regions + - Globalization strategies + +### Merit Functions + +19. **Fukushima, M.** (1992). "Equivalent differentiable optimization problems and descent methods for asymmetric variational inequality problems". _Mathematical programming_, 53(1), 99-110. + - Natural merit function φ(x) = 0.5||F(x)||² + - Descent properties + - Stationary point analysis + +20. **Geiger, C., & Kanzow, C.** (2002). "On the resolution of monotone complementarity problems". _Computational optimization and applications_, 5(2), 155-173. + - Merit function properties + - Exact penalty interpretation + - Global convergence + +### Subsystem Solvers + +21. **Saad, Y., & Schultz, M. H.** (1986). "GMRES: A generalized minimal residual algorithm for solving nonsymmetric linear systems". _SIAM Journal on scientific and statistical computing_, 7(3), 856-869. + - GMRES for general nonsymmetric systems + - Used for Fischer-Burmeister Jacobian + +22. **Hestenes, M. R., & Stiefel, E.** (1952). "Methods of conjugate gradients for solving linear systems". _Journal of research of the National Bureau of Standards_, 49(6), 409-436. + - Original conjugate gradient method + - Preconditioned CG for symmetric systems + +23. **Benzi, M.** (2002). "Preconditioning techniques for large linear systems: a survey". _Journal of computational Physics_, 182(2), 418-477. + - Incomplete Cholesky preconditioning + - Practical preconditioner choices + +### Gradient Descent for Warm Starting + +24. **Bertsekas, D. P.** (1999). _Nonlinear programming_ (2nd ed.). Athena scientific. + - Chapter 1.3: Gradient methods + - Convergence theory + - Step size selection + +25. **Polyak, B. T.** (1969). "The conjugate gradient method in extremal problems". _USSR Computational Mathematics and Mathematical Physics_, 9(4), 94-112. + - Heavy-ball method + - Momentum for acceleration + +### Applications to Physics Simulation + +26. **Silcowitz-Hansen, M., Erleben, K., & Niebe, S.** (2010). "A nonsmooth Newton method with applications to computer animation". In _MATHMOD 2009-6th Vienna International Conference on Mathematical Modelling_. + - Newton methods for contact problems + - Minimum map approach + - Warm-starting strategies + +27. **Kaufman, D. M., Tamstorf, R., Smith, B., Aubry, J. M., & Grinspun, E.** (2014). "Adaptive nonlinearity for collisions in complex rod assemblies". _ACM Transactions on Graphics (TOG)_, 33(4), 1-12. + - Newton methods for complex contact + - Active set prediction + - Adaptive strategies + +28. **Otaduy, M. A., Tamstorf, R., Steinemann, D., & Gross, M.** (2009). "Implicit contact handling for deformable objects". In _Computer Graphics Forum_, 28(2), 559-568. + - Newton methods for deformable contact + - Comparison with projection methods + +### Convergence Theory + +29. **Ulbrich, M.** (2011). _Semismooth Newton methods for variational inequalities and constrained optimization problems in function spaces_. SIAM. + - Advanced theory for Newton methods + - Mesh-independence for PDEs + - Superlinear convergence proofs + +30. **Facchinei, F., & Kanzow, C.** (1997). "Beyond monotonicity in regularization methods for nonlinear complementarity problems". _SIAM Journal on Control and Optimization_, 37(4), 1150-1161. + - Convergence without monotonicity + - Regularization techniques + - Robust algorithms + +--- + +**Navigation**: [← Projection Methods](04_projection-methods.md) | [Other Methods →](06_other-methods.md) diff --git a/docs/background/lcp/06_other-methods.md b/docs/background/lcp/06_other-methods.md new file mode 100644 index 0000000000000..f3e866027e7d2 --- /dev/null +++ b/docs/background/lcp/06_other-methods.md @@ -0,0 +1,537 @@ +# Other LCP Solver Methods + +**Navigation**: [← Newton Methods](05_newton-methods.md) | [Selection Guide →](07_selection-guide.md) + +## Overview + +This document covers additional LCP solver methods including Interior Point, Staggering, and specialized methods that don't fit into the main categories of pivoting, projection, or Newton methods. + +**All methods in this document are currently not implemented** but documented for future reference. + +## 1. Interior Point Method ❌ (Not Implemented) + +### Description + +Iterative method based on Kojima mapping that solves a relaxed complementarity problem while following a central path trajectory. + +### Problem Reformulation + +Instead of `x^T(Ax + b) = 0`, solve: + +``` +x^T(Ax + b) = mu (relaxed complementarity) +where mu > 0 is a small parameter +``` + +### Kojima Mapping + +``` +F(x, y, mu) = [ Ax - y + b ] + [ X*Y*e - mu*e ] +``` + +where: + +- X = diag(x), Y = diag(y) +- e = vector of ones +- mu = centering parameter + +### Algorithm + +``` +Initialize: x > 0, y > 0 +for iter = 1 to max_iter: + # Compute centering parameter + mu = sigma * x^T * y / n + where 0 < sigma < 1 + + # Solve Newton equation for Kojima mapping + [ A -I ] [ Δx ] = - [ Ax - y + b ] + [ Y X ] [ Δy ] [ X*Y*e - mu*e ] + + # Line search for step length + t_x = max{t | x + t*Δx >= (1-alpha)*x} + t_y = max{t | y + t*Δy >= (1-alpha)*y} + t = min(t_x, t_y) + + # Update + x = x + t*Δx + y = y + t*Δy + + # Reduce mu toward 0 + mu = mu / (iter + 1) +``` + +### Properties + +- **Time**: O(n³) per iteration (or O(n) with iterative solver) +- **Storage**: O(n²) (or O(n) with iterative solver) +- **Convergence**: Superlinear +- **Matrix Requirements**: None (very general) + +### Parameters + +- **sigma** (centering): 0 < sigma < 1 (typically 0.1-0.5) +- **alpha** (step control): 0 < alpha < 1 (typically 0.99) + +### Advantages/Disadvantages + +✅ Very robust - works for general LCPs +✅ Guaranteed interior iterates (x, y > 0) +✅ Theoretically well-founded (central path) +✅ Handles ill-conditioned problems +❌ O(n³) cost per iteration (expensive) +❌ More complex than projection methods +❌ Requires careful parameter tuning + +### Use Cases + +- Very ill-conditioned problems +- When other methods fail +- Problems requiring guaranteed feasibility +- When computational budget allows + +## 2. Staggering Methods ❌ (Not Implemented) + +### Description + +Partitions LCP into coupled sub-problems and solves them iteratively until convergence. + +### Typical Partition for Contact Problems + +``` +Variables partitioned into: + N - Normal forces + F - Friction forces + S - Slack variables + +Coupled LCPs: + A_NN * N + A_NF * F + A_NS * S = b_N + A_FN * N + A_FF * F + A_FS * S = b_F + A_SN * N + A_SF * F + A_SS * S = b_S +``` + +### Algorithm + +``` +Initialize: N, F, S +for iter = 1 to max_iter: + # Solve for normal forces (keeping F, S fixed) + N = solve_LCP(A_NN, b_N - A_NF*F - A_NS*S) + + # Solve for friction forces (keeping N, S fixed) + F = solve_QP(A_FF, b_F - A_FN*N - A_FS*S, bounds=[-mu*N, mu*N]) + + # Solve for slack variables (keeping N, F fixed) + S = solve_LCP(A_SS, b_S - A_SN*N - A_SF*F) + + # Check convergence + if ||change|| < epsilon: + break +``` + +### Semi-Staggering + +Only one iteration of staggering, used as warm-start: + +``` +# Initialize with PGS or zeros +x = initial_guess + +# One staggering iteration +N = solve_normal(...) +F = solve_friction(...) + +# Use as warm-start for Newton or other method +x_final = newton_method(x=[N, F]) +``` + +### Properties + +- **Time**: Depends on sub-solvers +- **Convergence**: Problem-dependent, no general theory +- **Effectiveness**: Can dramatically improve convergence + +### Sub-Solver Choices + +- **Normal forces**: 1D LCP (direct) or iterative +- **Friction forces**: QP solver (cone constraints) +- **Slack variables**: Direct elimination or iterative + +### Advantages/Disadvantages + +✅ Can use different solvers for different blocks +✅ Exploits problem structure +✅ Can accelerate convergence dramatically +❌ No convergence guarantees +❌ May not converge for all problems +❌ Requires problem-specific partitioning + +### Use Cases + +- Contact problems with normal/tangential separation +- Warm-starting Newton methods +- Problems with natural block structure +- When standard methods converge slowly + +## 3. Specialized Methods + +### 3.1 Shock-Propagation Method ❌ (Not Implemented) + +**Description**: Spatial blocking along gravity direction for fast shock wave propagation. + +**Algorithm**: + +``` +# Divide contacts into layers based on height +layers = compute_layers_by_gravity_direction(contacts) + +# Solve layer by layer from bottom to top +for layer in layers (bottom to top): + # Solve all contacts in this layer + for contact in layer: + solve_contact_LCP(contact) + + # Forces propagate to next layer +``` + +**Properties**: + +- Domain: Velocity-based BLCP +- Structure: Gravity-aligned layers +- Convergence: Fast for stacking scenarios + +**Use Cases**: + +- Tower/wall simulations +- Stacking scenarios +- Gravity-dominated scenes +- High-fidelity contact propagation + +### 3.2 Modified Proportioning with Reduced-Gradient Projections (MPRGP) ❌ + +**Description**: QP solver method using proportioning and reduced-gradient projections. + +**Application**: Fluid simulation (when LCP reformulated as QP) + +**Properties**: + +- Requires: A symmetric positive definite +- Convergence: Monotone descent +- Use: Fluid problems + +**Algorithm Sketch**: + +``` +while not converged: + # Proportioning step + Compute proportioning direction + + # Reduced-gradient projection + Project onto feasible set + + # Line search + Find step length + + # Update + x = x + t * direction +``` + +## 4. Blocked Jacobi ❌ (Not Implemented) + +### Description + +Jacobi splitting applied to blocks (similar to Blocked Gauss-Seidel but with Jacobi update). + +### Algorithm + +``` +# All blocks updated in parallel +for iter = 1 to max_iter: + parallel for each block i: + r_i = b_i + sum(A_{ij} * x_j^k, j != i) + x_i^{k+1} = SolveSubLCP(A_{ii}, r_i) +``` + +### Properties + +- **Parallelization**: Fully parallel (all blocks independent) +- **Convergence**: Slower than BGS, faster than scalar Jacobi +- **Use**: Parallel computing, GPU + +## Comparison Summary + +| Method | Convergence | Complexity | Robustness | Parallelization | +| ----------------- | ----------- | ---------- | ---------- | --------------- | +| Interior Point | Superlinear | O(n³) | Very High | No | +| Staggering | Variable | Depends | Medium | No | +| Shock-Propagation | Linear | O(n) | Medium | Limited | +| MPRGP | Monotone | O(n²) | High | No | +| Blocked Jacobi | Linear | O(n·b³) | Medium | Yes | + +## Implementation Priority + +### Low Priority (Specialized Use Cases) + +- **Staggering**: For specific contact problem structures +- **Shock-Propagation**: For gravity-dominated scenarios + +### Very Low Priority (Niche Applications) + +- **Interior Point**: When all else fails +- **MPRGP**: Fluid-specific applications +- **Blocked Jacobi**: Parallel hardware optimization + +## When to Use These Methods + +### Interior Point + +**Use when**: + +- All other methods fail to converge +- Very ill-conditioned problems +- Robustness is paramount + +**Avoid when**: + +- Real-time constraints (too expensive) +- Simpler methods work fine + +### Staggering + +**Use when**: + +- Natural problem partition exists +- Need to warm-start Newton methods +- Standard methods converge slowly + +**Avoid when**: + +- Problem has no natural structure +- Convergence guarantees needed + +### Specialized Methods + +**Use when**: + +- Problem matches specific structure (e.g., gravity layers) +- Domain-specific optimizations beneficial + +**Avoid when**: + +- General-purpose solver needed +- Problem doesn't match assumptions + +## External Solver Integration + +### QP Solvers for Symmetric PD Matrices + +When A is symmetric PD, LCP can be reformulated as QP: + +``` +minimize: 0.5 * x^T * A * x + x^T * b +subject to: x >= 0 +``` + +**Available Solvers**: + +- **MOSEK** (Commercial): Very robust, supports large-scale +- **CPLEX** (Commercial): IBM optimizer, industry standard +- **LANCELOT** (Free): Academic use, Fortran-based +- **SQP**: Sequential Quadratic Programming +- **SNOPT**: Sparse Nonlinear Optimizer + +**Integration Strategy**: + +```cpp +// Wrapper for external QP solver +class QPSolverWrapper { + bool solve(const MatrixXd& A, const VectorXd& b, VectorXd* x) { + // Convert to QP format + // Call external solver + // Convert solution back + } +}; +``` + +## Future Work + +### Multigrid Methods + +Potential for O(n) convergence with multilevel hierarchy: + +``` +Coarse grid -> Medium grid -> Fine grid +``` + +**Benefits**: + +- Faster convergence than single-level +- Preserve robustness of splitting methods + +**Challenges**: + +- Complex implementation +- Problem-specific grid structure + +### Adaptive Methods + +Dynamic method selection based on problem characteristics: + +``` +if (well_conditioned): + use PGS +elif (ill_conditioned): + use Pivoting or Interior Point +elif (small_problem): + use Newton +``` + +## References + +### Interior Point Methods + +1. **Kojima, M., Megiddo, N., Noma, T., & Yoshise, A.** (1991). _A unified approach to interior point algorithms for linear complementarity problems_. Springer Science & Business Media. + - Kojima mapping and central path + - Path-following algorithms + - Theoretical foundations + +2. **Wright, S. J.** (1997). _Primal-dual interior-point methods_. SIAM. + - Comprehensive treatment of interior point methods + - Chapter 9: Complementarity problems + - Implementation details + +3. **Kojima, M., Megiddo, N., & Noma, T.** (1989). "Homotopy continuation methods for nonlinear complementarity problems". _Mathematics of Operations Research_, 14(3), 404-416. + - Path-following for NCP/LCP + - Centering parameter selection + - Global convergence + +4. **Monteiro, R. D., & Pang, J. S.** (1996). "A potential reduction Newton method for constrained equations". _SIAM Journal on Optimization_, 9(3), 729-754. + - Potential reduction approach + - Newton systems in interior point + - Applications to LCP + +5. **Ye, Y.** (1997). _Interior point algorithms: theory and analysis_. John Wiley & Sons. + - Theory of interior point methods + - Polynomial-time algorithms + - Linear and nonlinear complementarity + +### Staggering Methods + +6. **Kaufman, D. M., Sueda, S., James, D. L., & Pai, D. K.** (2008). "Staggered projections for frictional contact in multibody systems". _ACM Transactions on Graphics (TOG)_, 27(5), 1-11. + - Staggering for normal/tangential forces + - Convergence analysis + - Application to cloth and rigid bodies + +7. **Otaduy, M. A., & Gross, M.** (2007). "Interactive design and analysis of deformable materials using force density meshes". _ACM Transactions on Graphics (TOG)_, 26(3), 14. + - Staggering for constraint satisfaction + - Warm-starting techniques + +8. **Tournier, M., Nesme, M., Gilles, B., & Faure, F.** (2015). "Stable constrained dynamics". _ACM Transactions on Graphics (TOG)_, 34(4), 1-10. + - Semi-staggering for stability + - Partitioning strategies + +### Shock-Propagation Method + +9. **Guendelman, E., Bridson, R., & Fedkiw, R.** (2003). "Nonconvex rigid bodies with stacking". _ACM transactions on graphics (TOG)_, 22(3), 871-878. + - Original shock propagation method + - Layer-based spatial decomposition + - Gravity-aligned blocking + +10. **Pabst, S., Koch, A., & Straßer, W.** (2010). "Fast and scalable CPU/GPU collision detection for rigid and deformable surfaces". In _Computer Graphics Forum_, 29(5), 1605-1612. + - Parallel shock propagation + - Spatial layering on GPU + +11. **Tonge, R., Benevolenski, F., & Voroshilov, A.** (2012). "Mass splitting for jitter-free parallel rigid body simulation". _ACM Transactions on Graphics (TOG)_, 31(4), 1-8. + - Related parallel propagation + - Domain decomposition for contacts + +### MPRGP (Modified Proportioning with Reduced-Gradient Projections) + +12. **Dostál, Z.** (2009). _Optimal quadratic programming algorithms: with applications to variational inequalities_. Springer Science & Business Media. + - MPRGP algorithm + - Proportioning and projections + - Applications to contact and fluid problems + +13. **Dostál, Z., & Schöberl, J.** (2005). "Minimizing quadratic functions subject to bound constraints with the rate of convergence and finite termination". _Computational Optimization and Applications_, 30(1), 23-43. + - Convergence analysis of MPRGP + - Finite termination properties + +14. **Dostál, Z., Kozubek, T., Markopoulos, A., & Brzobohatý, T.** (2010). "Scalable FETI algorithms for large 3D multibody contact problems". In _Parallel Processing and Applied Mathematics_, 312-321. Springer. + - MPRGP for large-scale contact + - FETI domain decomposition + +### Blocked Jacobi + +15. **Adams, L. M.** (1982). "Iterative algorithms for large sparse linear systems on parallel computers". _NASA Technical Memorandum_, 83267. + - Block Jacobi for parallelization + - Applications to sparse systems + +16. **Schwarz, H. A.** (1870). "Über einige Abbildungsaufgaben". _Journal für die reine und angewandte Mathematik_, 70, 105-120. + - Original Schwarz alternating method + - Historical foundation for domain decomposition + +### External QP Solvers + +17. **MOSEK ApS** (2023). "MOSEK Optimization Suite". https://www.mosek.com/ + - Commercial QP/LCP solver + - Interior point methods + - Large-scale optimization + +18. **IBM ILOG** (2023). "CPLEX Optimizer". https://www.ibm.com/products/ilog-cplex-optimization-studio + - Commercial optimization suite + - Simplex and barrier methods + - Mixed-integer support + +19. **Conn, A. R., Gould, N. I., & Toint, P. L.** (2000). _Trust region methods_. SIAM. + - LANCELOT algorithm (Chapter 17) + - Large-scale nonlinear optimization + - Free for academic use + +20. **Gill, P. E., Murray, W., & Saunders, M. A.** (2005). "SNOPT: An SQP algorithm for large-scale constrained optimization". _SIAM review_, 47(1), 99-131. + - Sequential Quadratic Programming + - Sparse nonlinear optimization + - Applications to complementarity + +### Multigrid Methods (Future Work) + +21. **Brandt, A.** (1977). "Multi-level adaptive solutions to boundary-value problems". _Mathematics of computation_, 31(138), 333-390. + - Original multigrid method + - V-cycle and W-cycle + - Potential for O(n) LCP solvers + +22. **Hackbusch, W.** (1985). _Multi-grid methods and applications_. Springer Science & Business Media. + - Comprehensive multigrid theory + - Applications to various PDEs + - Adaptation to LCP + +23. **Gratton, S., Sartenaer, A., & Toint, P. L.** (2008). "Recursive trust-region methods for multiscale nonlinear optimization". _SIAM Journal on Optimization_, 19(1), 414-444. + - Multiscale optimization + - Trust regions on different scales + - Related to multigrid ideas + +### Applications and Comparisons + +24. **Erleben, K., Silcowitz-Hansen, M., & Niebe, S.** (2017). "Numerical methods for linear complementarity problems in physics-based animation". _Synthesis Lectures on Computer Graphics and Animation_, 11(2), 1-159. + - Comprehensive survey including all methods + - Performance comparisons + - Implementation recommendations + +25. **Tasora, A., Negrut, D., & Anitescu, M.** (2008). "Large-scale parallel multi-body dynamics with frictional contact on the graphical processing unit". _Proceedings of the Institution of Mechanical Engineers, Part K: Journal of Multi-body Dynamics_, 222(4), 315-326. + - GPU implementations of various methods + - Parallel Jacobi and staggering + - Performance analysis + +26. **Mazhar, H., Heyn, T., Negrut, D., & Tasora, A.** (2013). "Using Nesterov's method to accelerate multibody dynamics with friction and contact". _ACM Transactions on Graphics (TOG)_, 34(3), 1-14. + - Acceleration techniques + - Comparison with standard methods + - Applications to granular materials + +27. **Todorov, E., Erez, T., & Tassa, Y.** (2012). "MuJoCo: A physics engine for model-based control". In _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems_, 5026-5033. IEEE. + - PGS and Newton-Raphson in MuJoCo + - Practical solver selection + - Real-world applications + +--- + +**Navigation**: [← Newton Methods](05_newton-methods.md) | [Selection Guide →](07_selection-guide.md) diff --git a/docs/background/lcp/07_selection-guide.md b/docs/background/lcp/07_selection-guide.md new file mode 100644 index 0000000000000..59393cb84dd5b --- /dev/null +++ b/docs/background/lcp/07_selection-guide.md @@ -0,0 +1,523 @@ +# LCP Solver Selection Guide + +**Navigation**: [← Other Methods](06_other-methods.md) | [Overview](02_overview.md) + +## Quick Decision Tree + +``` +START + | + ├─ Real-time/Interactive? ──YES──> PGS or PSOR + | (when implemented) + | + ├─ High accuracy needed? ──YES──> Newton Methods or Pivoting + | (Dantzig/Lemke available now) + | + ├─ Large problem (n>1000)? ──YES──> NNCG or PGS + | (when implemented) + | + ├─ Ill-conditioned? ──YES──> Pivoting (Dantzig/Lemke) + | or Interior Point + | + ├─ Contact problem? ──YES──> BGS or ODELCPSolver + | (ODELCPSolver available now) + | + └─ Default ──> Start with Dantzig or Lemke + (currently implemented) +``` + +## Detailed Selection by Use Case + +### 1. Real-Time Physics Simulation + +**Recommended (when implemented)**: PGS > PSOR > BGS +**Currently Available**: Dantzig, Lemke, ODELCPSolver + +**Rationale**: + +- Need O(n) per-iteration cost +- Acceptable approximate solutions +- 50-100 iterations typical +- Predictable performance + +**Configuration**: + +``` +Method: PGS (when implemented) +Max iterations: 50-100 +Tolerance: 1e-4 to 1e-6 +Warm start: Previous time-step solution +``` + +**Current Workaround**: + +```cpp +// Use ODELCPSolver for contact problems +ODELCPSolver solver; +solver.Solve(A, b, &x, numContacts, mu, numDir, true); + +// Or Dantzig for bounded problems +SolveLCP(n, A, x, b, w, nub, lo, hi, findex); +``` + +### 2. High-Accuracy Off-Line Simulation + +**Recommended**: Newton > Pivoting > Interior Point +**Currently Available**: Dantzig ✅, Lemke ✅ + +**Rationale**: + +- Need accuracy 1e-8 to 1e-12 +- Computational budget allows +- 5-20 iterations typical for Newton +- Exact solutions from pivoting + +**Configuration (Future Newton)**: + +``` +Method: Minimum Map Newton +Max iterations: 20-50 +Tolerance: 1e-8 to 1e-12 +Warm start: PGS (10 iterations) +Subsolver: GMRES with tolerance 0.1*||H|| +``` + +**Current Best**: + +```cpp +// Dantzig for bounded problems +SolveLCP(n, A, x, b, w, nub, lo, hi, findex); + +// Lemke for standard LCP +Lemke(M, q, &z); +``` + +### For Contact Mechanics with Friction + +**Recommended**: ODELCPSolver (uses Dantzig) > Dantzig directly +**Currently Available**: ODELCPSolver ✅, Dantzig ✅ + +**Rationale**: + +- Natural block structure (per-contact) +- Friction cone constraints +- Normal/tangential coupling +- ODELCPSolver handles formulation automatically + +**Configuration**: + +```cpp +// Current: Use ODELCPSolver (wrapper around Dantzig) +dart::math::ODELCPSolver solver; +bool success = solver.Solve( + A, b, &x, + numContacts, // Number of contact points + mu, // Friction coefficient + numDir, // Friction directions (0, 2, or 4) + true // Use internal Dantzig solver +); + +// Or use Dantzig directly with friction indices +int findex[n]; +// Set up findex for friction cone +dart::math::SolveLCP(n, A, x, b, w, nub, lo, hi, findex); +``` + +**Future BGS (when implemented)**: + +``` +Per-contact blocks with: + - Normal: 1D direct solve + - Friction: 2D/3D geometric solver + - Pyramid: 4D small iterative +``` + +### 4. Large-Scale Problems (n > 1000) + +**Recommended (when implemented)**: NNCG > PGS > Newton+Iterative +**Currently Available**: Dantzig, Lemke (limited scalability) + +**Rationale**: + +- O(n) per-iteration essential +- Matrix-free implementations +- Sparse matrix exploitation + +**Future Configuration**: + +``` +Method: NNCG +Max iterations: 100-500 +Restart: Every 20 iterations +Warm start: Previous solution or zeros +``` + +**Current Approach**: + +``` +# For very large problems, consider: +# 1. Problem decomposition +# 2. Using external solvers (MOSEK, CPLEX) +# 3. Approximation techniques +``` + +### 5. Ill-Conditioned Problems + +**Recommended**: Pivoting > Interior Point +**Currently Available**: Dantzig ✅, Lemke ✅ + +**Rationale**: + +- Robust to conditioning +- Handle large mass ratios +- Numerical stability + +**Current Solution**: + +```cpp +// Dantzig is robust for ill-conditioned problems +SolveLCP(n, A, x, b, w, nub, lo, hi, findex); + +// Or Lemke +Lemke(M, q, &z); +validate(M, z, q); // Always validate +``` + +**When Pivoting Fails**: + +``` +Try: +1. Problem reformulation +2. Regularization (add small diagonal) +3. Scaling/preconditioning +4. External QP solver (if A is symmetric PD) +``` + +### 6. Parallel/GPU Computing + +**Recommended (when implemented)**: Jacobi > Red-Black GS > Blocked Jacobi +**Currently Available**: None (sequential methods only) + +**Rationale**: + +- Embarrassingly parallel updates +- GPU-friendly operations +- No data dependencies + +**Future Configuration**: + +``` +Method: Jacobi or Red-Black GS +Max iterations: 100-200 (more than PGS) +Parallelization: GPU kernels +Block size: 32-256 threads per block +``` + +## Method Comparison Matrix + +### Computational Cost + +| Use Case | Best Method | Per-Iter Time | Iterations | Total Time | Available Now | +| ------------- | ------------ | ------------- | ---------- | ---------- | ------------- | +| Real-time | PGS | O(n) | 50-100 | O(50n) | ❌ | +| Real-time | Dantzig | O(n³) | 1 | O(n³) | ✅ | +| High accuracy | Newton | O(n³)\* | 5-20 | O(20n³)\* | ❌ | +| High accuracy | Dantzig | O(n³) | 1 | O(n³) | ✅ | +| Contact | BGS | O(nb³) | 50-100 | O(50nb³) | ❌ | +| Contact | ODELCPSolver | - | - | - | ✅ | +| Large-scale | NNCG | O(n) | 20-200 | O(200n) | ❌ | + +\* With iterative subsolver + +### Accuracy vs Speed Trade-off + +``` +Higher Accuracy ─────────────────────────> Lower Accuracy +Slower ─────────────────────────> Faster + +Pivoting ─> Newton ─> Interior Point ─> NNCG ─> BGS ─> PGS ─> Jacobi +(exact) (1e-10) (1e-8) (1e-6) (1e-4) (1e-3) (1e-2) + +✅ Available: Dantzig, Lemke +❌ Future: All others +``` + +### Robustness vs Efficiency + +``` +More Robust ─────────────────────────> Less Robust +Slower ─────────────────────────> Faster + +Pivoting ─> Interior Point ─> Newton ─> BGS ─> PGS ─> Jacobi + +✅ Available: Dantzig, Lemke +❌ Future: All others +``` + +## Problem Size Guidelines + +| Problem Size | Recommended Method | Currently Available | +| ---------------- | ------------------------ | -------------------- | +| n < 10 | Direct 2D/3D or Pivoting | Dantzig ✅, Lemke ✅ | +| 10 ≤ n < 100 | Pivoting or Newton | Dantzig ✅, Lemke ✅ | +| 100 ≤ n < 1000 | PGS, BGS, or Newton | Dantzig ✅ (slow) | +| 1000 ≤ n < 10000 | NNCG or PGS | None (need PGS) | +| n ≥ 10000 | NNCG or specialized | None | + +## Conditioning Guidelines + +| Matrix Condition | Recommended Method | Currently Available | +| -------------------- | ------------------------ | -------------------- | +| Well-conditioned | Any method | All ✅ | +| Moderate | PGS, Newton, Pivoting | Dantzig ✅, Lemke ✅ | +| Ill-conditioned | Pivoting, Interior Point | Dantzig ✅, Lemke ✅ | +| Very ill-conditioned | Pivoting only | Dantzig ✅, Lemke ✅ | + +## Implementation Roadmap Impact + +### Current State (2025-11-22) + +Available solvers: + +- ✅ **Dantzig**: BLCP with bounds, friction support +- ✅ **Lemke**: Standard LCP +- ✅ **ODELCPSolver**: Contact problems wrapper + +**Best Practices Now**: + +```cpp +// For bounded problems with friction +SolveLCP(n, A, x, b, w, nub, lo, hi, findex); + +// For standard LCP +Lemke(M, q, &z); + +// For contact problems +ODELCPSolver solver; +solver.Solve(A, b, &x, numContacts, mu, numDir, true); +``` + +### Phase 1: Core Iterative (Priority) + +When PGS/PSOR are implemented: + +- ✅ Real-time simulation becomes viable +- ✅ Most contact problems solvable interactively +- ✅ O(n) iteration cost achieved + +### Phase 2: Blocked Methods + +When BGS is implemented: + +- ✅ Better contact force accuracy +- ✅ Natural per-contact structure +- ✅ Sub-solvers for small blocks + +### Phase 3: Advanced Iterative + +When NNCG is implemented: + +- ✅ Large-scale problems solvable +- ✅ Better convergence than PGS +- ✅ Mass ratio handling improved + +### Phase 4: Newton Methods + +When Newton methods are implemented: + +- ✅ High accuracy achievable +- ✅ Superlinear convergence +- ✅ Off-line simulation improved + +## Common Pitfalls and Solutions + +### Pitfall 1: Using Expensive Method for Real-Time + +**Problem**: Using Dantzig (O(n³)) for real-time with n > 100 + +**Solution**: + +``` +Current: Use ODELCPSolver or limit problem size +Future: Use PGS or PSOR when implemented +``` + +### Pitfall 2: Expecting High Accuracy from Iterative + +**Problem**: Running PGS for 10000 iterations expecting 1e-10 + +**Solution**: + +``` +PGS: Good for 1e-4 to 1e-6 +For 1e-10: Use Newton or Pivoting (Dantzig ✅ available) +``` + +### Pitfall 3: Not Warm-Starting + +**Problem**: Starting Newton or iterative from x=0 every time + +**Solution**: + +```cpp +// Use previous solution +x_init = x_previous_timestep; + +// Or warm-start with few PGS iterations (when available) +``` + +### Pitfall 4: Wrong Method for Problem Structure + +**Problem**: Using scalar PGS for contact problems + +**Solution**: + +``` +Current: Use ODELCPSolver ✅ +Future: Use BGS (better per-contact structure) +``` + +### Pitfall 5: Ignoring Convergence Monitoring + +**Problem**: Running fixed iterations without checking convergence + +**Solution**: + +```cpp +// Always check multiple criteria +bool converged = (residual < abs_tol) || + (relative_change < rel_tol) || + (iterations >= max_iter); + +// Monitor divergence +if (merit_function > gamma * previous_merit) { + // Handle divergence +} +``` + +## Parameter Tuning Guidelines + +### PGS/PSOR (when implemented) + +``` +max_iterations: + - Real-time: 50-100 + - Accuracy: 500-1000 + +tolerance: + - Real-time: 1e-4 to 1e-6 + - Accuracy: 1e-6 to 1e-8 + +relaxation (PSOR only): + - Start: 1.0 (PGS) + - Tune: 1.2-1.5 + - Under-relax if unstable: 0.8-0.95 +``` + +### Newton Methods (when implemented) + +``` +max_iterations: 20-50 + +tolerance: + - Standard: 1e-8 + - High accuracy: 1e-12 + +line_search: + - alpha: 1e-4 (sufficient decrease) + - beta: 0.5 (step reduction) + +subsolver_tolerance: + - Initial: 0.1 * ||H|| + - Final: 0.01 * ||H|| +``` + +### Dantzig (currently available) + +```cpp +// Early termination for approximate solutions +bool earlyTermination = true; // For faster approximate solve + +// Set bounds carefully +lo[i] <= 0 // Must be non-positive +hi[i] >= 0 // Must be non-negative + +// Friction indices +findex[i] = -1; // No friction dependency +findex[i] = j; // Depends on x[j] for friction cone +``` + +## Troubleshooting Guide + +### Method Not Converging + +**For Dantzig/Lemke (current)**: + +``` +1. Check matrix properties +2. Try problem reformulation +3. Add regularization: A += epsilon * I +4. Scale problem: divide A, b by max(|A|) +``` + +**For Future Iterative Methods**: + +``` +1. Increase max_iterations +2. Try different starting point +3. Check matrix conditioning +4. Try relaxation (PSOR with lambda < 1) +5. Fall back to pivoting +``` + +### Solution is Infeasible + +``` +1. Validate input: A, b satisfy LCP structure +2. Check bounds: lo <= 0 <= hi +3. Verify complementarity: x^T(Ax+b) ≈ 0 +4. Try different solver (Dantzig vs Lemke) +``` + +### Performance is Poor + +``` +Current (with Dantzig/Lemke): +1. Limit problem size (n < 100) +2. Use ODELCPSolver for contacts +3. Reduce contact points +4. Simplify collision geometry + +Future (with PGS/Newton): +1. Use appropriate method for problem size +2. Enable warm-starting +3. Matrix-free implementations +4. Exploit sparsity +``` + +## Summary Recommendations + +### Current State (Until Phase 1 Complete) + +| Scenario | Use | Notes | +| ---------------------- | ----------------------------- | ---------------------------- | +| Contact with friction | ODELCPSolver | Best option now | +| Bounded variables | Dantzig | Supports bounds and friction | +| Standard LCP | Lemke | Simple and robust | +| Large problems (n>100) | Limit size or external solver | Current methods O(n³) | +| Real-time (n>50) | Reduce problem size | O(n³) too expensive | + +### Future State (After Implementation) + +| Scenario | Primary | Backup | Notes | +| --------------- | ------- | -------------- | ------------------ | +| Real-time | PGS | PSOR | 50-100 iterations | +| Contact | BGS | PGS | Per-contact blocks | +| High accuracy | Newton | Dantzig | 5-20 iterations | +| Large-scale | NNCG | PGS | >1000 variables | +| Ill-conditioned | Dantzig | Interior Point | Most robust | +| Parallel | Jacobi | Red-Black GS | GPU-friendly | + +--- + +**Navigation**: [← Other Methods](06_other-methods.md) | [Overview](02_overview.md) diff --git a/docs/dev_tasks/README.md b/docs/dev_tasks/README.md index a600aeac2c285..7071b2b023356 100644 --- a/docs/dev_tasks/README.md +++ b/docs/dev_tasks/README.md @@ -39,7 +39,7 @@ Documentation for development tasks in DART. - ❌ **Performance numbers** - These change with optimizations - ✅ **DO**: High-level design decisions with "why" rationale - ✅ **DO**: Architectural patterns used (e.g., "hybrid approach", "threshold-based") -- ✅ **DO**: Directory pointers (e.g., "see `dart/math/lcp/Dantzig/`") +- ✅ **DO**: Directory pointers (e.g., "see `dart/math/lcp/dantzig/`") **Remember**: Code is the source of truth. Documentation explains _why_, code shows _how_. diff --git a/docs/onboarding/constraints.md b/docs/onboarding/constraints.md index 89d24771fad46..a7df0f27505b0 100644 --- a/docs/onboarding/constraints.md +++ b/docs/onboarding/constraints.md @@ -42,7 +42,7 @@ This document provides a comprehensive analysis of the constraint subsystem in t - PgsBoxedLcpSolver.hpp/cpp - PGS boxed LCP solver **Dantzig Solver Implementation:** -Modern C++20 template-based implementation in `dart/math/lcp/Dantzig/` with two key design decisions: +Modern C++20 template-based implementation in `dart/math/lcp/dantzig/` with two key design decisions: - **PivotMatrix:** Hybrid architecture combining Eigen storage with pointer-based O(1) row swapping - **Hybrid SIMD:** Threshold-based strategy switching between raw pointers and Eigen SIMD based on problem size