\documentclass{article}

\usepackage{palatino}

\usepackage{amsmath}
\usepackage{amsfonts}
\usepackage[utf8]{inputenc}
\usepackage[colorlinks=true]{hyperref}

% macros
\newcommand{\LL}{\mathcal{L}}
\newcommand{\half}{\frac{1}{2}}
\newcommand{\norm}[1]{\left|\left|#1\right|\right|}
\newcommand{\dd}{d}
\newcommand{\ddd}[2]{\frac{\partial #1}{\partial #2}}
\newcommand{\block}[1]{\left(#1\right)}
\newcommand{\mat}[1]{ \begin{pmatrix} #1 \end{pmatrix} }
\newcommand{\inv}[1]{#1^{-1}}
\newcommand{\argmin}[1]{\underset{#1}{\textrm{argmin}}}

\begin{document}
\title{Compliant Reference}
\author{Maxime Tournier}
\date{\today}
\maketitle
%
\begin{abstract}
  Quick notes about the mathematical framework used in Compliant. We
  review the time-stepping scheme, kinematic constraint handling,
  potential forces as compliant constraints, and KKT systems.
\end{abstract}
%
\section*{TODO}
\begin{itemize}
  \item Code cheat sheet
  \item finish sections :-)
  \item references
  \item wikipedia links
  \item introduction ?
\end{itemize}

\section{Lagrangian Dynamics}
Given an $n$-dimensional state space $Q$, we consider a Lagrangian
$\LL$ defined as usual:
%
\begin{equation}
\LL(q, v) = \half v^TMv - V(q)
\end{equation}
%
where $V(q)$ a the potential energy of the form:
%
\begin{equation}
V(q) = \half \norm{g(q)}_N^2
\end{equation}
%
for a matrix norm $N$ in the suitable deformation space $Im(g)$. To be
fully general, we include a Rayleigh dissipation function $\half
v^TDv$, where $D$ is PSD. The Euler-Lagrange equations of motions are,
in this case:
%
\begin{equation}
 \frac{d}{dt}\ddd{\LL}{v} = \ddd{\LL}{q} - \ddd{D}{v} 
\end{equation}
%
For simplicity, we restrict to the case where $M$ is constant, which
yields:
%
\begin{equation}
  \label{eq:dynamics}
  M \dot{v} = -\nabla_q V - D v
\end{equation}
%
\section{Time Stepping}
%
We now discretize time using a fixed time step $h$. We use forward
differences for $\dot{v}$ as follows:
%
\begin{equation}
  h\ \dot{v}_{k+1} \approx v_{k+1} - v_k
\end{equation}
%
Equation \eqref{eq:dynamics} becomes: 
%
\begin{equation}
  M v_{k+1} = M v_k - h \block{\nabla_q V_{k+1} + D v_{k+1}}
\end{equation}
%
Or, calling $p_k = M v_k$ the momentum of the system and $f_k =
-\nabla_q V_k$ the potential forces:
%
\begin{equation}
  \block{M + h\ D} v_{k+1} = p_k + h f_{k+1}
\end{equation}
%
In order to control how implicit our integrator is on forces, we
introduced a blending parameter $\alpha \in [0, 1]$ as follows:
%
\begin{equation}
  \block{M + h \alpha D} v_{k+1} = p_k - h(1 - \alpha) D v_k + h \block{(1 - \alpha) f_k + \alpha f_{k+1}}
\end{equation}
%
We linearize the above using the Hessian of the potential energy:
%
\begin{equation}
\nabla_q V_{k+1} \approx \nabla_q V_k + \nabla_q^2 V_k \block{q_{k+1} - q_k}
\end{equation}
%
so that:
%
\begin{equation}
  (1 - \alpha) f_k + \alpha f_{k+1} = f_k - \alpha \nabla_q^2 V_k \block{q_{k+1} - q_k}
\end{equation}
%
Finally, we use the following position time-stepping scheme:
%
\begin{equation}
  q_{k+1} = q_k + h \block{ (1-\beta) v_k + \beta v_{k+1}}
\end{equation}
%
where $\beta \in [0, 1]$ is again a blending parameter.
%
Letting the stiffness matrix be $K = \nabla_q^2 V_k$ %
\footnote{
%
  This is the opposite of the stiffness matrix as defined in SOFA,
  \emph{i.e.} $\nabla f(q) = -\nabla_q^2 V$. I find it easier to work with
  PSD matrices.} and $d_k = -D v_k$ be the damping forces, we put
everything together as:
%
\begin{equation}
  \label{eq:time-stepping}
  \block{M + h\alpha D + h^2 \alpha \beta K} v_{k+1} = 
  p_k + h \block{ f_k + (1 - \alpha) d_k} - \alpha ( 1 - \beta) h^2 K v_k 
\end{equation}
%
(and yes, this is very ugly). Fortunately, everyone would use the much
friendlier, fully implicit scheme $\alpha = \beta = 1$ as follows:
%
\begin{equation}
   \block{M + hD + h^2K} v_{k+1} = p_k + h f_k
\end{equation}
%
\section{Response Matrix, Net Momentum}
%
From now on, we will refer to the \emph{response matrix} defined as
follows:
%
\begin{equation}
  \label{eq:response-matrix}
  W = \block{M + h\alpha D + h^2 \alpha \beta K}
\end{equation}
%
We will also refer to the \emph{net momentum} of the system at time
step $k$:
%
\begin{equation}
  \label{eq:net-momentum}
  c_k = p_k + h \block{ f_k + (1 - \alpha) d_k} - \alpha ( 1 - \beta) h^2 K v_k
\end{equation}
%
The time-stepping scheme \eqref{eq:time-stepping} thus involves solving
the following linear system:
%
\begin{equation}
  W v_{k+1} = c_k
\end{equation}
%
The numerical solvers for time-stepping will be described in section
\ref{sec:solvers}.
%
\section{Constraints}
\label{sec:constraints}
%
We now introduce holonomic constraints of the form:
%
\begin{equation}
  g(q) = 0
\end{equation}
%
where $g$ again maps kinematic DOFs to a suitable deformation space
$Im(g)$. Such constraints are satisfied by introducing constraint
forces $J^T\lambda$, where $J = \dd g$ is the constraint Jacobian
matrix, and $\lambda$ are the \emph{Lagrange multipliers}:
%
\begin{align}
  \label{eq:constrained-dynamics}
  M \dot{v} &= -\nabla_q V - D v + J^T \lambda\\
  g(q) &= 0
\end{align}
%
Again, we discretize time and $\dot{v}$ as follows:
%
\begin{align}
  M v_{k+1} &= p_k + h \block{f_{k+1} + d_{k+1} + J_{k+1}^T\lambda_{k+1}} \\
  g\block{q_{k+1}} &= 0 
\end{align}
%
And again:
\begin{equation}
g( q_{k+1}) = g_k + h J_k \block{ (1 - \beta) v_k + \beta v_{k+1}}  
\end{equation}
%
At this point we become lazy so we approximate constraints as
\emph{affine} functions, meaning that $J_{k+1} \approx J_k$, otherwise
computing the constraint Hessian $\dd^2 g$ would be too costly, and
would result in a non-linear system to solve anyways (\emph{i.e.}
with terms involving $v_{k+1}^T\dd^2g_k \lambda_{k+1}$). As we will
see later, such approximation is consistent with our treatment of
potential forces as compliant constraints. The time-discrete system is
then:
%
\begin{align}
  M v_{k+1} &= p_k + h \block{f_{k+1} + d_{k+1} + J_k^T\lambda_{k+1}} \\
  J_k v_{k+1} &= -\frac{g_k}{\beta h} - \frac{1 - \beta}{\beta} J_k v_k \label{eq:constraintvalue}
\end{align}
% 
At this point, we \emph{could} introduce an implicit blending between
$\lambda_k$ and $\lambda_{k+1}$, but this would result in unneeded
complication as $\lambda_{k+1}$ would need to be computed anyways. The
blended force would then be:
\[ J_k^T\block{ (1-\alpha) \lambda_k + \alpha \lambda_{k+1} } \] which
simply offsets $\lambda_{k+1}$ with respect to $\lambda_k$. We will
thus happily ignore such blending. The final linear system to solve
(for $v_{k+1}$ and $\lambda_{k+1}$) becomes:
%
\begin{align}
\label{eq:constrained-integrator}
  W v_{k+1} - J_k^T \lambda_{k+1} &= c_k  \\
  J_k v_{k+1} &= -b_k
\end{align}
where $b_k = \frac{g_k}{\beta h} + \frac{1 - \beta}{\beta} J_k
v_k$. Before leaving, note the sneaky accounting of $h$ inside
$\lambda_{k+1}$.
%
\section{Compliance}
\label{sec:compliance}
%
We will now establish a connection between elastic forces and
constraint forces through a compliance matrix. Remember that we
defined potential energy as:
%
\begin{equation}
  V(q) = \half \norm{g(q)}_N^2
\end{equation}
%
where $g$ maps kinematic DOFs to an appropriate deformation space
$Im(g)$. This means the potential forces are:
%
\begin{equation}
  f = - \nabla_q V = J^T N g(q)
\end{equation}
%
Now, the Hessian or stiffness matrix is :
%
\begin{equation}
  K = \nabla^2_q V = \dd J^T N g(q) + J^T N J
\end{equation}
%
As in section \ref{sec:constraints}, we are too lazy for computing
second derivatives so we approximate deformation mappings as affine
maps, meaning that $\dd J \approx 0$. We are left with the following
response matrix \eqref{eq:response-matrix}:
%
\begin{equation}
  W = \block{M + h\alpha D + h^2 \alpha \beta J_k^T N J_k}
\end{equation}
%
The net momentum \eqref{eq:net-momentum} becomes:
%
\begin{equation}
  c_k = p_k + h (1 - \alpha) d_k - h J_k^TN \block{ g_k + h \alpha ( 1 - \beta) J_k v_k}
\end{equation}
%
If we write the potential force as $J_k^T \lambda_{k+1}$, we have: 
\begin{equation}
  \lambda_{k+1} = -N\block{h^2 \alpha \beta J_k v_{k+1} + h g_k + h^2 \alpha (1 - \beta) J_k v_k}
\end{equation}
It turns out that this system can be rewritten as a \emph{larger} system:
%
\begin{equation}
  \mat{M + h \alpha D & -J_k \\
    J_k & \frac{\inv{N}}{h^2 \alpha \beta} } \mat{v_{k+1} \\ \lambda_{k+1}} = \mat{p_k + h (1 - \alpha) d_k \\ -\frac{g_k}{h \alpha \beta} - \frac{( 1 - \beta)}{\beta} J_k v_k }
\end{equation}
%
(pffew !) One can notice that this system is \emph{almost} exactly the
one we obtained with kinematic constraints in
\eqref{eq:constrained-integrator}, with $\alpha = 1$, with the
exception of the $\inv{N}$ term in the $(2, 2)$ block. We call matrix
$C = \inv{N}$ the \emph{compliance} matrix. We see that kinematic
constraints simply correspond to a zero compliance matrix, \emph{i.e.}
infinite stiffness, as one would intuitively expect. It can be checked
that taking $\alpha$ into account for constraint forces produces the
same system with $C = 0$. (TODO) We now recall the main results for
the unified elastic/constraint treatment:
%
\subsection*{Linear System}
\begin{equation}
  \label{eq:time-stepping-kkt}
  \mat{ W & -J \\
    -J & -\frac{C}{h^2 \alpha \beta} } \mat{v \\ \lambda} = \mat{c_k \\ b_k}
\end{equation}
%
\subsection*{Response Matrix}
%
\begin{equation}
  W = M + h\alpha D
\end{equation}
%
\subsection*{Net Momentum}
%
\begin{equation}
c_k = p_k + h (1 - \alpha) d_k  
\end{equation}
%
\subsection*{Constraint Value}
%
\begin{equation}
  b_k = \frac{g_k}{h \alpha \beta} + \frac{( 1 - \beta)}{\beta} J_k v_k
\end{equation}
%
TODO pourquoi c'est cool: transition seamless entre contraintes et
elasticite, traitement unifié contraintes/elasticité, conditionnement
pour les sytemes très raides. par contre ca fait des systèmes plus gros.
%
\section{KKT Systems, Compliance and Relaxation}
%
At this point, it is probably a good idea to introduce a few notions
on saddle-point (or KKT) systems. Such systems are (for now, linear)
systems of the form:
%
\begin{equation}
 \label{eq:kkt-system}
 \mat{Q & -A^T \\-A & 0} \mat{x\\\lambda} = \mat{c\\b}
\end{equation}
%
KKT systems typically arise from the Karush-Kuhn-Tucker (hence the
name) optimality conditions of constrained optimization problems. For
instance, the KKT system \eqref{eq:kkt-system} corresponds to the
following equality-constrained Quadratic Program (QP):
%
\begin{equation}
  \label{eq:constrained-optimization}
  \underset{Ax + b = 0}{\textrm{argmin}} \quad \half x^T Q x - c^T x
\end{equation}
%
The KKT system summarizes the optimality conditions for the following
unconstrained function (also called the \emph{Lagrangian}, this time in the
context of optimization):
%
\begin{equation}
  \LL(x, \lambda) \quad = \quad \half x^T Q x - c^T x \quad - \quad \block{A x + b}^T \lambda
\end{equation}
%
and whose critical points (in fact, saddle-points) solve the
constrained problem \eqref{eq:constrained-optimization}. As one can
see, the constrained integrator \eqref{eq:constrained-integrator} is
an example of such KKT systems:
\begin{equation}
  \mat{W & -J\\-J & 0} \mat{v \\ \lambda} = \mat{c \\ b}
\end{equation}
%
where we dropped subscripts for the sake of readability. As symmetric
\emph{indefinite} systems, they tend to be more difficult to solve
than positive definite ones: CG and Cholesky $LDL^T$ factorizations
may breakdown on such systems. However, when the $(1, 1)$ block (here,
matrix $Q$) is invertible, one can use \emph{pivoting} to obtain the
following smaller system:
%
\begin{align}
  x &= \inv{Q}\block{c - A^T \lambda} \\
  -A x &= A\inv{Q}A^T \lambda - A\inv{Q} c = b \\
  \label{eq:schur-complement}
  A\inv{Q}A^T\lambda &= b - A \inv{Q} c
\end{align}
%
This smaller system \eqref{eq:schur-complement} is known as the
\emph{Schur} complement of the KKT system. It is always SPD as long as
$A^T$ is full column-rank, but requires to invert $Q$ which might be
costly in practice. The Schur complement system also corresponds to an
(unconstrained) optimization problem:
%
\begin{equation}
  \argmin{\lambda}\quad \half \lambda^T S \lambda - s^T \lambda
\end{equation}
%
where $S = A \inv {Q} A^T$ is the Schur complement, and $s = b +
A\inv{Q}c$. The minimized quantity has a physical interpretation:
typically, holonomic constraints minimize the total kinetic energy. If
we now consider systems with compliance, such as:
%
\begin{equation}
  \label{eq:compliant-kkt-system}
  \mat{Q & -A^T \\-A & -D} \mat{x\\\lambda} = \mat{c\\b}
\end{equation}
%
we see that they correspond to the following Lagrangian:
%
\begin{equation}
  \LL(x, \lambda) \quad = \quad \half x^T Q x - c^T x \quad - 
  \quad \block{A x + b}^T \lambda \quad - \quad \half \lambda^T D \lambda
\end{equation}
%
One can check that the Schur complement becomes $S + D$, and
corresponds to:
%
\begin{equation}
  \argmin{\lambda}\quad \half \lambda^T S \lambda - s^T \lambda \quad + 
  \quad \half \lambda^T D \lambda
\end{equation}
%
We see that $D$ acts as a form of numerical damping on the resolution
of constraints, by biasing the solution of the constrained system
towards zero. It is a form of \emph{constraint
  relaxation}. Physically, elastic forces minimize a mix between the
kinetic energy of the system and the $D$-norm of the forces
$\lambda$. Under their most general form, KKT systems include
unilateral and complementarity constraints. We will develop these in
more details in section \ref{sec:unilateral-constraints}.
%
\section{Geometric Stiffness}
%
For both constraint and elastic forces, we happily neglected
second-order derivatives of the deformation mapping $g$. In the case
of elastic forces, this resulted in a nice stiffness matrix of the
form:
%
\begin{equation}
  \nabla_q^2 V(q) \approx J^T N J = K
\end{equation}
%
which enabled us to treat elastic forces as a compliant kinematic
constraints. But what about the second order terms $\tilde{K}=(\dd
J)^T N g(q)$ ? First of all, when the configuration space is a vector
space $Q$ and $g$ is $\mathcal{C}^2$-continuous, the Schwarz' theorem
ensures that $\nabla_q^2 V$ is symmetric, hence so is $\tilde{K}$. We
call $\tilde{K}$ the \emph{geometric stiffness}, as it is induced by
the variation of the deformation mapping $g$. We do not know whether
in the general case, it is possible to factor \emph{both} $K$ and
$\tilde{K}$ as:
%
\begin{equation}
  \nabla_q^2 V(q) = K + \tilde{K} \overset{?}{=} J^T\block{N + \tilde{N}}J
\end{equation}
%
even though some specific examples exist (\emph{e.g.} mass spring
systems). Of course, it is always possible to apply a Cholesky
factorization directly on $\nabla_q^2 V(q)$ as:
%
\begin{equation}
  \nabla_q^2 V(q) = LDL^T
\end{equation}
%
and get back on our feet, but this would be highly inefficient in
practice. Therefore, unless an ad-hoc derivation provides the needed
factorization, we are left with only one alternative: treat the
geometric stiffness as a regular stiffness instead of compliance.
%
\section{Unilateral Constraints}
\label{sec:unilateral-constraints}
%
We now consider \emph{unilateral constraints} (inequality) instead of
bilateral ones:
%
\begin{equation}
  \label{eq:unilateral-constraint}
  g(q) \geq 0
\end{equation}
%
Examples of such constraints include non-penetration constraints, or
angular limits for an articulated rigid body. Just like in the
bilateral case, the constraints are enforced by the addition of
constraint forces $J^T \lambda$, satisfying velocity constraints of
the form:
%
\begin{equation}
  J v_{k+1} \geq -b_k
\end{equation}
%
obtained by differentiation of \eqref{eq:unilateral-constraint},
according to the time-stepping scheme. Furthermore, reaction forces
must satisfy additional requirements known as the Signorini
conditions:
%
\begin{itemize}
\item constraint forces must be repulsive: $\lambda_{k+1} \geq 0$
\item constraint forces don't act when the constraint is inactive, and
  conversely:
  \[ J_k v_{k+1} > -b_k \Rightarrow \lambda_{k+1} = 0, \quad \lambda_{k+1} > 0 \Rightarrow J_k v_{k+1} = -b_k \]
\end{itemize}
%
Intuitively, a non-penetration contact force is not allowed to push
bodies further apart when they are already separating. The
requirements on constraint forces are summarized by the following
\emph{complementarity} constraint:
%
\begin{equation}
  \label{eq:complementarity-constraint}
  0 \leq J_k v_{k+1} + b_k \ \bot \ \lambda_{k+1} \geq 0 
\end{equation}
%
The time-stepping scheme with constraint forces is, as before:
%
\begin{equation}
  \label{eq:time-stepping-unilateral}
  W v_{k+1} -J_k^T \lambda_{k+1} = c_k
\end{equation}
%
It turns out that the complementarity constraints
\eqref{eq:complementarity-constraint} together with time-stepping
equation \eqref{eq:time-stepping-unilateral} form the KKT conditions
of the following \emph{inequality-constrained} Quadratic Program:
%
\begin{equation}
  v_{k+1} = \argmin{J_kv + b_k\  \geq\  0} \quad \half v^TWv - c_k^T v
\end{equation}
%
Therefore, time-stepping in the presence of unilateral constraints can
be readily solved by a general QP solver. As in the bilateral case,
when $W$ is easily invertible, it is possible to compute the Schur
complement in order to obtain an equivalent, but smaller problem:
%
\begin{equation}
  \label{eq:lcp}
  0 \leq J\inv{W}J^T \lambda_{k+1} + b_k - A\inv{W} c_k\ \bot \ \lambda_{k+1} \geq 0 
\end{equation}
%
(TODO: check formula) Such problem is known as a \emph{Linear
  Complementarity Problem} (LCP) and can be solved by various
algorithms, some of which will be presented in section
\ref{sec:solvers}.
%
\section{Stabilization}
%

\section{Restitution}
%
TODO: velocity constraints for contact with restitution (rigid),
mention Generalized Reflections
%
\section{Numerical Solvers}
\label{sec:solvers}
%
TODO: CG, Cholesky, Minres, GS, PGS, Sequential Impulses


\section{Code Cheat Sheet}

A quick reminder of who does what where in the code:

\subsection{CompliantImplicitSolver}

\begin{itemize}
\item Performs KKT system assembly
\item Builds right-hand sides (correction/dyamics) computation
\item Integrate system solution, update graph and stuff
\end{itemize}

\subsection{Right-hand side computation}

Lots of cases:
\begin{itemize}
\item Bilateral (default), unilateral, friction constraint type
\item Elastic constraints, kinematic constraints (can be stabilized) (default), restitution constraints, \ldots ?
\end{itemize}

A constraint has a BaseConstraintValue constraint value (if not, the
default is assigned) that produces a correct constraint violation term
(the right-hand side for constraints in the KKT system) depending on
the correction/dynamics phase.

Elastic constraints have zero rhs during correction, and -error / dt
during dynamics.

Stabilized constraints (Stablization component) correct the error
during stabilization, but have zero rhs during dynamics (holonomic
kinematic constraints)

Restitution constraints: stabilize when velocity is small and when the constraint is not violated,
otherwise flip relative velocity.

Depending on the type (bilateral, unilateral, friction), the
constraint also has an associated Projector, that you can find in the
KKT system.

\subsection{KKTSolver}
Solves a KKT system (AssemledSystem), for both correction/dynamics cases.



\section{Alternative formulations}

Alternative formulations allow more complex projective constraints (e.g. velocity constraints) where the velocity formulation can only handle the fixed projective constraint.
Howerver their corresponding linear system seems harder to resolve for constraints.
Note that stabilization is always performed in velocity.

\subsection{Acceleration}

By considering $v_{k+1} = v_k + h a $, eq. (\ref{eq:time-stepping}) becomes :

\begin{equation}
  \block{M + h\alpha D + h^2 \alpha \beta K} a = 
  f_k - D v_k - \alpha h K v_k
\end{equation}

and eq. (\ref{eq:constraintvalue}) becomes :

\begin{align}
  J_k a &= \frac{ -\frac{g_k}{\alpha h} - J_k v_k } {\beta h}
\end{align}


\subsection{Velocity variation}

By considering $v_{k+1} = v_k + dv $, eq. (\ref{eq:time-stepping}) becomes :

\begin{equation}
  \block{M + h\alpha D + h^2 \alpha \beta K} dv = 
  h f_k - h D v_k - \alpha h^2 K v_k
\end{equation}

and eq. (\ref{eq:constraintvalue}) becomes :

\begin{align}
  J_k dv &= \frac{ -\frac{g_k}{\alpha h} - J_k v_k } {\beta}
\end{align}


\end{document}

