Merge branch 'main' of https://github.com/dojo-sim/Dojo.jl into main

Author: thowell

docs/src/interior_point.md

@@ -1 +1,45 @@
-# Algorithm
+# Algorithm
+
+### Overview
+To simulate the system forward in time, we need to solve a Nonlinear Complementarity Problem (NCP) at each time step. To efficiently and reliably satisfy the NCP, we developed a custom primal-dual interior-point solver for NCPs with cone constraints and quaternions. The algorithm is largely based upon Mehrotra's predictor-coorector algorithm, while borrowing practical numerical features from CVXOPT to handle cones and non-Euclidean optimization to handle quaternions. We also introduce heuristics that further improve reliability and overall performance of the solver for our simulation-step NCPs.
+
+The primary advantages of this algorithm are the correction to the classic Newton step, which can greatly reduce the iterations required by the solver (often halving the total number of iterations), and feedback on the problem's central-path parameter that helps avoid premature ill-conditioning and adaptively drives the complementarity violation to zero in order to reliably simulate hard contact.
+
+### Problem formulation
+The solver aims to satisfy instantiations of the following problem:
+
+$$\text{find}\quad  x, y, z \\
+\text{subject to}\quad  c(x, y, z; \theta) = 0, \\
+y^{(i)} \circ z^{(i)} = \kappa \mathbf{e}, \quad i = 1,\dots,n, \\
+y^{(i)}, z^{(i)} \in \mathcal{K}, \quad i = 1,\dots, n,$$
+
+with decision variables $x \in \mathbf{R}^k$ and $y, z \in \mathbf{R}^m$, equality-constraint set $c : \mathbf{R}^k \times \mathbf{R}^m \times \mathbf{R}^m \times \mathbf{R}^l \rightarrow \mathbf{R}^h$, problem data $\theta \in \mathbf{R}^l$; and where $\mathcal{K}$ is the Cartesian product of $n$ total positive-orthant and second-order cones. The variables are partitioned: $x = (x^{(1)}, \dots, x^{(p)})$, where $i = 1$ are Euclidean variables and $i = 2, \dots, p$ are each quaternion variables; and $y = (y^{(1)}, \dots, y^{(n)})$, $z = (z^{(1)}, \dots, z^{(n)})$, where $j = 1$ is the positive-orthant and the remaining $j = 2, \dots, n$ are second-order cones. For convenience, we denote $w = (x, y, z)$.
+
+The algorithm aims to satisfy a sequence of relaxed problems with $\kappa > 0$ and $\kappa \rightarrow 0$ in order to reliably converge to a solution of the original problem (i.e., $\kappa = 0$). This continuation approach helps avoid premature ill-conditioning and is the basis for numerous convex and non-convex general-purpose interior-point solvers.
+
+### Violation metrics:
+Two metrics are used to measure progress:
+The constraint violation,
+
+$$r_{\text{vio}} = \| c(w; \theta) \|_{\infty},$$
+
+and complementarity violation,
+
+$$b_{\text{vio}} = {\text{max}}_i \{\| y^{(i)} \circ z^{(i)} \|_{\infty}\}.$$
+
+The NCP is considered solved when $r_{\text{vio}} < r_{\text{tol}}$ and $b_{\text{vio}} < b_{\text{tol}}$.
+!!! info "solver options"
+    Both `r_tol` and `b_tol` are options that can easily be accessed and modified via [`SolverOptions`](@ref).
+
+### Newton Steps
+The main loop of the solver performs Newton's method on the equality-constraint set $c$ and the bilinear constraints. The solver typically converges in about 10 iterations.
+!!! info "solver options"
+    The maximal number of Newton's iterations `max_iter` can be set via [`SolverOptions`](@ref).
+
+### Line Search
+Newton's method provides a search direction, then we perform a line search along this search direction to detemine the step length $\alpha$. We use a backtracking line search that accepts the step whenever it decreases $c_{\text{vio}}$ or $b_{\text{vio}}$.
+The line search starts with a step $\alpha=1$, if the step acceptance conditions are not met the step is decreased geometrically:
+$$\alpha \leftarrow \alpha \times s $$. The line search takes at most `max_ls` backtracking steps.
+
+!!! info "solver options"
+    The scaling parameter $s$ is called `ls_scale` and the maximum number of line search iteration `max_ls` can be set via [`SolverOptions`](@ref).

docs/src/linearized_friction.md

@@ -16,9 +16,9 @@ $$\text{minimize}_{\beta} \quad [v^T  -v^T] \beta, \\
 \text{subject to} \quad \beta^T \mathbf{1} \leq \mu \gamma, \\
 \beta \geq 0,$$
 
-which satisfies the LCP formulation, is instead solved. Here, the friction cone is linearized (Fig. \ref{friction_cones}) and the friction vector, $\beta \in \mathbf{R}^{4}$, is correspondingly overparameterized and subject to additional non-negative constraints \cite{stewart1996implicit}.
+which satisfies the LCP formulation, is instead solved. Here, the friction cone is linearized and the friction vector, $\beta \in \mathbf{R}^{4}$, is correspondingly overparameterized and subject to additional non-negative constraints.
 
-The optimality conditions of \eqref{mdp_linear} and constraints used in the LCP are:
+The optimality conditions of the above problem and constraints used in the LCP are:
 
 $$[v^T  -v^T]^T + \psi \mathbf{1} - \eta = 0, \\
 \mu \gamma -\beta^T \textbf{1} \geq 0,\\

docs/src/nonlinear_friction.md

@@ -25,7 +25,8 @@ The second-order-cone product is:
 $$\beta \circ \eta = (\beta^T \eta, \beta_{(1)} \eta_{(2:n)} + \eta_{(1)} \beta_{(2:n)}),$$
 
 and,
-$$\mathbf{e} = (1, 0, \dots, 0) \label{soc_identity},$$
+
+$$\mathbf{e} = (1, 0, \dots, 0),$$
 
 is its corresponding identity element. Friction is recovered from the solution: $b = \beta^*_{(2:3)}$. The benefits of this model are increased physical fidelity and fewer optimization variables, without substantial increase in computational cost.
 

docs/src/solver_options.md

@@ -1 +1,15 @@
-# Solver Options
+# Solver Options
+
+The solver we implemented has several options all accessible via [`SolverOptions`](@ref). Here is a list describing their impact on the solver's behavior, their typical values and if they need to be tuned.
+
+
+| option                  | min $\cdot$ default $\cdot$ max values | effect | require tuning |
+| ----------------------- | -------------------------------------- | ------ | -------------- |
+| `rtol`                  | $10^{-6}$ $\cdot$ $10^{-4}$ $\cdot$ $10^{-2}$ | larger leads to faster solve (usually takes the same value as `btol`) | rarely |
+| `btol`                  | $10^{-6}$ $\cdot$ $10^{-4}$ $\cdot$ $10^{-2}$ | larger results in smoothed contact dynamics and faster solve | rarely |
+| `ls_scale`              | $0.3$ $\cdot$ $0.5$ $\cdot$ $0.8$             | larger potentially increase step size at the cost of more residual evaluations | never |
+| `max_ls`                | $5$ $\cdot$ $10$ $\cdot$ $15$                 | larger allows for taking smaller steps | never |
+| `undercut`              | $2$ $\cdot$ $+\infty$ $\cdot$ $+\infty$       | larger is more robust but can generate stiffer gradients | rarely |
+| `no_progress_max`       | $3$ $\cdot$ $3$ $\cdot$ $5$                   | smaller will increase the undercut faster | never |
+| `no_progress_undercut`  | $3$ $\cdot$ $10$ $\cdot$ $100$                | larger will increase the undercut faster | never |
+| `verbose`               | true $\cdot$ false $\cdot$ false              | printing the status of the solver | often |