$\newcommand{\deriv}[2][]{\dfrac{\partial #1}{\partial #2}} \newcommand{\tderiv}[2][]{\tfrac{\partial #1}{\partial #2}} \newcommand{\fderiv}[2][]{\dfrac{d #1}{d #2}} \newcommand{\derivII}[3][]{\dfrac{\partial^2 #1}{\partial #2 \partial #3}} \newcommand{\tderivII}[3][]{\tfrac{\partial^2 #1}{\partial #2 \partial #3}} \newcommand{\derivIII}[4][]{\dfrac{\partial^3 #1}{\partial #2 \partial #3 \partial #4}} \newcommand{\tderivIII}[4][]{\tfrac{\partial^3 #1}{\partial #2 \partial #3 \partial #4}} \newcommand{\derivIV}[5][]{\dfrac{\partial^4 #1}{\partial #2 \partial #3 \partial #4 \partial #5}} \newcommand{\half}{\tfrac{1}{2}} % Macro for 1/2 \newcommand{\ud}{\mathrm{d}} \newcommand{\op}{\circ} \newcommand{\dq}{\dot{q}} \newcommand{\ddq}{\ddot{q}} \newcommand{\dt}{\Delta t}$

Discrete Dynamics and Variational Integrators

The variational integrator simulates a discrete mechanical systems. Given a known state (time \(t_{k-1}\), configuration \(q_{k-1}\), and discrete momentum \(p_{k-1}\)) and inputs (force inputs \(u_{k-1}\) and next kinematic configuration \(\rho_{k-1}\)), the integrator finds the next state:

\[(t_{k-1}, q_{k-1}, p_{k-1}), (u_{k-1}, \rho_{k-1}) \Rightarrow (t_k, q_k, p_k)\]

The integrator also finds the discrete constraint force variable \(\lambda_{k-1}\).

The variational integrator finds the next state by numerically solving the constrained Discrete Euler-Langrange (DEL) equation for \(q_k\) and \(\lambda_{k-1}\):

\[ \begin{align}\begin{aligned}p_{k-1} + D_1L_d(q_{k-1}, q_k, t_{k-1}, t_{k-1}) + f_d^-(q_{k-1}, q_k, u_{k-1}, t_{k-1}, t_k) - Dh^T(q_{k-1})\dot \lambda_{k-1}\\h(q_k) = 0\end{aligned}\end{align} \]

and then calculating the new discrete momentum:

\[p_k = D_2L_d(q_{k-1}, q_k, t_{k-1}, t_k)\]

In trep, we simplify notation by letting \(k=2\), so that we consider \(t_1\), \(q_1\), and \(p_1\) to be the previous state, and \(t_2\), \(q_2\), and \(p_2\) to be the new or current state.

\[ \begin{align}\begin{aligned}L_2 = L_d(q_1, q_2, t_1, t_2)\\f_2^\pm = f_d^\pm(q_1, q_2, u_1, t_1, t_2)\\h_1 = h(q_1)\\h_2 = h(q_2)\end{aligned}\end{align} \]