Dynamical system control

28/03/2021 updated on 02/02/2023  │ Notes
numerical analysis
control

\(\newcommand{\T}{\mathsf{\small{T}}}\newcommand{\d}{\mathrm{d}}\)

Notations

Let \(\mathbf{q},\mathbf{\dot q},\mathbf{\ddot q}\) be the generalized coordinates, velocities, and accelerations. The vector \(\mathbf{a}\) represent the controllable parameters, which are to be read as the activation parameters. The superscripts involving \(t\) will be used to denote time points such that \(\mathbf{q}^{t+\tau} = \mathbf{q}(t+\tau)\) represents the discrete value of \(\mathbf{q}\) at time \(t+\tau\), while subscript involving \(n\) as in \(\mathbf{q}_n^t\) will represent the value of \(\mathbf{q}^t\) at some iteration \(n\). The symbol \(\tau\) represents the timestep.

The constrained equation of motion is given as

\[ \mathbf{M}\mathbf{\ddot q} +\mathbf{f}=\mathbf{G}^\T\boldsymbol\lambda \] where \(\mathbf{M}\) is the mass matrix, assumed constant, \(\mathbf{f}\) is the force vector which contains internal and external forces \(\mathbf{f}=\mathbf{f}_\text{int}-\mathbf{f}_\text{ext}\), and \(\boldsymbol\lambda\) are the Lagrange multipliers which enforce the respect of the constraints in the system. For the sake of simplicity, the constraints are assumed to be holonomic, i.e. \(\mathbf{g}\big(\mathbf{q}(t)\big)=0\), and enforced at velocity level via index reduction, i.e. \(\frac{\mathrm{d}\mathbf{g}}{\mathrm{d}\mathbf{q}}\mathbf{\dot q}=\mathbf{G}\mathbf{\dot q}=0\)

Discretization

The equation of motion is made explicit as such \[ \mathbf{M}\mathbf{\ddot q}(t)+\mathbf{f}(\mathbf{q}(t),\mathbf{\dot q}(t),\mathbf{a}(t))=\mathbf{G}^\T\!(\mathbf{q}(t))\boldsymbol{\lambda}(t) \] The derivation is performed based on the implicit Euler scheme. The iterative procedure resulting from the Newton’s method yields \[ \begin{aligned} \mathbf{H}_n\Delta\mathbf{\dot{q}}&= -\mathrm{R}_{\mathbf{\dot q},n}+\tau\left(\mathbf{K}+\frac{\partial\mathbf{G}_n}{\partial\mathbf{q}}\right)\mathrm{R}_{\mathbf{q},n} + \mathbf{G}_n^\T(\boldsymbol\lambda^{t+\tau}_n+\Delta\boldsymbol\lambda) -\tau\mathbf{A}_n\Delta\mathbf{a} \\ \mathbf{\dot{q}}^{t+\tau}_{n+1}&=\mathbf{\dot{q}}^{t+\tau}_n+\Delta\mathbf{\dot{q}} \\ \boldsymbol{\lambda}^{t+\tau}_{n+1} &= \boldsymbol{\lambda}^{t+\tau}_{n} + \Delta\boldsymbol\lambda \\ \mathbf{a}^{t+\tau}_{n+1} &= \mathbf{a}^{t+\tau}_n + \Delta\mathbf{a} \\ \mathbf{q}^{t+\tau}_{n+1} &= \mathbf{q}^t+ \tau\mathbf{\dot q}^{t+\tau}_{n+1} \\ \mathbf{f}^{t+\tau}_{n+1} &= \mathbf{f}\left(\mathbf{q}^{t+\tau}_{n+1},\mathbf{\dot q}^{t+\tau}_{n+1},\mathbf{a}^{t+\tau}_{n+1}\right) \end{aligned} \] with \[ \begin{aligned} &\mathbf{H} = \mathbf{M}+\tau\mathbf{D}+\tau^2\left(\mathbf{K}+\frac{\partial\mathbf{G}}{\partial\mathbf{q}}\boldsymbol\lambda_n\right)\\ &\mathbf{D} = \frac{\partial\mathbf{f}}{\partial\mathbf{\dot q}},\, \mathbf{K} = \frac{\partial\mathbf{f}}{\partial\mathbf{q}},\,\mathbf{A} = \frac{\partial\mathbf{f}}{\partial\mathbf{a}}\\ &\mathrm{R}_{\mathbf{\dot q},n} = \mathbf{M}\left(\mathbf{\dot q}^t-\mathbf{\dot q}^{t+\tau}_n\right)-\tau\mathbf{f}^{t+\tau}_n\\ &\mathrm{R}_{\mathbf{q},n} = \mathbf{q}^t-\mathbf{q}^{t+\tau}_n+\tau\mathbf{\dot q}^{t+\tau}_n \end{aligned} \]

wherein the iteration subscript for matrices have been omitted for the sake of clarity. Since the residual on position is projected at every iteration, which means \(\mathrm{R}_\mathbf{q}=0\), the system can be further simplified as \[ \begin{aligned} \mathbf{H}\Delta\mathbf{\dot{q}}&= -\mathrm{R}_{\mathbf{\dot q},n}+ \mathbf{G}^\T(\boldsymbol\lambda^{t+\tau}_n+\Delta\boldsymbol\lambda) -\tau\mathbf{A}\Delta\mathbf{a} \\ \end{aligned} \]

which provides the discretized residual \[ \mathbf{Q} = \mathbf{H}\Delta\mathbf{\dot{q}}+\mathrm{R}_{\mathbf{\dot q},n} + \tau\mathbf{A}\Delta\mathbf{a} - \mathbf{G}^\T(\boldsymbol\lambda^{t+\tau}_n+\Delta\boldsymbol\lambda) = 0 \]

Direct and Adjoint formulation

At each iteration, one seeks a new set of activation that enable to reach a desired state. \[ \arg\min_{\Delta\mathbf{a}} \;L(\mathbf{\dot q}^{t+\tau}) \quad \text{s.t.} \quad \mathbf{Q}=0 \] The state is implicitly a function of the control variables. The gradient of the objective needed for gradient-based optimization is derived as \[ \begin{aligned} \frac{\d\mathrm{L}}{\d\mathbf{a}} &= \cancelto{0}{\frac{\partial\mathrm{L}}{\partial\mathbf{a}}} + \frac{\partial\mathrm{L}}{\partial\mathbf{\dot q}} \frac{\partial\mathbf{\dot q}}{\partial\mathbf{a}} \\ &=\frac{\partial\mathrm{L}}{\partial\mathbf{\dot q}}\left(\frac{\partial\mathbf{Q}}{\partial\mathbf{\dot q}}\right)^{\!\!-1}\frac{\partial\mathbf{Q}}{\partial\mathbf{a}} \\ &=\rlap{\underbrace{\frac{\partial\mathrm{L}}{\partial\mathbf{\dot q}}\left(\frac{\partial\mathbf{Q}}{\partial\mathbf{\dot q}}\right)^{\!\!-1}}_\text{Adjoint}} \phantom{\frac{\partial\mathrm{L}}{\partial\mathbf{\dot q}}}\overbrace{\phantom{\left(\frac{\partial\mathbf{Q}}{\partial\mathbf{\dot q}}\right)^{\!\!-1}} \frac{\partial\mathbf{Q}}{\partial\mathbf{a}}}^\text{Direct} \\ &= \frac{\partial\mathrm{L}}{\partial\mathbf{\dot q}} \mathbf{H}^{-1}\tau\mathbf{A} \end{aligned} \] By computing the gradient with the direct approach, one needs to perform \(n_a\) solve, the amount of control variables, while the adjoint method allows to perform only \(n_L\) solve, equivalent to the amount of cost functions considered in the objective.

Abstract view and discrete derivatives

The system of equations to solve at each iteration of the Newton’s algorithm can be abstracted into the following representation \[ \begin{bmatrix} \mathbf{H} & \phantom{-}\mathbf{G}^\T \\ \mathbf{G} & -\mathbf{E}\phantom{\T} \end{bmatrix}\begin{bmatrix} {\phantom{-\tau}\Delta\mathbf{\dot q}} \\ -\tau\Delta\boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \mathbf{h} \\ \mathbf{g} \end{bmatrix} \]

where factors have shifted around to present a symmetric matrix. Incorporating the knowledge of the activation parameter lead to the following refined description \[ \begin{bmatrix} \mathbf{H} & \phantom{-}\mathbf{G}^\T \\ \mathbf{G} & -\mathbf{E}\phantom{\T} \end{bmatrix}\begin{bmatrix} {\phantom{-\tau}\Delta\mathbf{\dot q}} \\ -\tau\Delta\boldsymbol{\lambda} \\\end{bmatrix} = \begin{bmatrix} \mathbf{h}_a + \mathbf{h}_{\not a}\\ \mathbf{g} \end{bmatrix} \] where the right hand side is decomposed into a dependent and independent function of activation. The Schur complement of the system yields \[ \left(-\mathbf{E}-\mathbf{G}\mathbf{H}^{-1}\mathbf{G}^\T\right) \left(-\tau\Delta\boldsymbol\lambda\right) = \mathbf{g} - \mathbf{G}\mathbf{H}^{-1}\left(\mathbf{h}_{\not a}+\mathbf{h}_a\right) \] and the increment in velocity is obtained via backsubstitution \[ \begin{aligned} &\mathbf{H}\Delta\mathbf{\dot q} = \mathbf{h}_{\not a}+\mathbf{h}_a +\tau\mathbf{G}^\T\Delta\boldsymbol\lambda \\ &\mathbf{H}\Delta\mathbf{\dot q} = \mathbf{h}_{\not a}+\mathbf{h}_a +\tau\mathbf{G}^\T\left( \mathbf{E}+\mathbf{G}\mathbf{H}^{-1}\mathbf{G}^\T\right)^{-1} \left(\mathbf{g} - \mathbf{G}\mathbf{H}^{-1}\left(\mathbf{h}_{\not a}+\mathbf{h}_a\right)\right) \end{aligned} \] The variation with respect to activation follows from the derivation of previous equations \[ \left(-\mathbf{E}-\mathbf{G}\mathbf{H}^{-1}\mathbf{G}^\T\right) \left(-\tau\frac{\partial\boldsymbol\lambda}{\partial \mathbf{a}} \right) = - \mathbf{G}\mathbf{H}^{-1}\frac{\partial\mathbf{h}_a}{\partial\mathbf{a}} \] and \[ \mathbf{H}\frac{\partial\mathbf{\dot q}}{\partial\mathbf{a}} =\frac{\partial\mathbf{h}_a}{\partial\mathbf{a}} -\tau\mathbf{G}^\T\left( \mathbf{E}+\mathbf{G}\mathbf{H}^{-1}\mathbf{G}^\T\right)^{-1} \mathbf{G}\mathbf{H}^{-1}\frac{\partial\mathbf{h}_a}{\partial\mathbf{a}} \] Note that in that derivation, the use of such \(\frac{\partial\mathbf{\dot q}}{\partial\mathbf{a}}\) in a controller leads to a constraint-aware solution: the variation of activation needed to reach a new state will be null if a constraint prevents to do so.

Target velocity (or tracking cost)

Finding the activation that would provide the desired position or velocity can be formulated as a smooth quadratic term using a least square formulation. \[ \begin{align*} & L = \tfrac{1}{2}\left\Vert \mathbf{\dot q}^{*}-\mathbf{P}\mathbf{\dot q}^{t+\tau} \right\Vert_2^2 \\ \end{align*} \] where \(\mathbf{P}\)​ projects the global state to the target space and \(\mathbf{\dot q}^*\)​ is the target velocity. The gradient and hessian of the cost function are \[ \begin{align*} & \frac{\d L}{\d \mathbf{a}} = -(\mathbf{\dot q}^{*}-\mathbf{P}\mathbf{\dot q}^{t+\tau})^\T \mathbf{P}\frac{\partial \mathbf{\dot q}}{\partial\mathbf{a}}\\ & \frac{\d L}{\d \mathbf{aa}} = \frac{\partial \mathbf{\dot q}}{\partial\mathbf{a}}\mathbf{P}^\T\mathbf{P}\frac{\partial \mathbf{\dot q}}{\partial\mathbf{a}} -(\mathbf{\dot q}^{*}-\mathbf{P}\mathbf{\dot q}^{t+\tau})^\T\frac{\partial \mathbf{\dot q}}{\partial\mathbf{aa}} \end{align*} \] Note that the higher order derivative \(\frac{\partial\mathbf{\dot q}}{\partial\mathbf{aa}}\) is often neglected. The detailed form can be obtained by plugging in the following \[ \mathbf{H}\frac{\partial\mathbf{\dot q}}{\partial\mathbf{a}} = -\tau\mathbf{A}+\tau\mathbf{G}^\T\frac{\partial\boldsymbol\lambda}{\partial\mathbf{a}} \]

Target force from constraint

Similarly, forces (or impulses) resulting from constraints in the dynamical system can be converted to a smooth quadratic energy term using a least square formulation. \[ \begin{align*} & L = \tfrac{1}{2}\left\Vert \tau\boldsymbol\lambda^{*}-\tau\mathbf{P}_\lambda\boldsymbol\lambda \right\Vert_2^2 \\ \end{align*} \] where \(\mathbf{P}_{\lambda}\) projects the global state to the target space and \(\boldsymbol\lambda^*\) is the target force. The gradient and hessian of the cost function is as follows \[ \begin{align*} & \frac{\d L}{\d \mathbf{a}} = -\tau^2(\boldsymbol\lambda^{*}-\mathbf{P}_\lambda\boldsymbol\lambda)^\T \mathbf{P}_\lambda\frac{\partial \boldsymbol\lambda}{\partial\mathbf{a}}\\ & \frac{\d L}{\d \mathbf{aa}} = \tau\frac{\partial\boldsymbol\lambda}{\partial\mathbf{a}}\mathbf{P}_\lambda^\T\mathbf{P}_\lambda\frac{\partial\boldsymbol\lambda}{\partial\mathbf{a}}\tau -\tau^2(\boldsymbol\lambda^{*}-\mathbf{P}_\lambda\boldsymbol\lambda^{t+\tau})^\T\frac{\partial\boldsymbol\lambda}{\partial\mathbf{aa}} \end{align*} \]

Note that the higher order derivative \(\frac{\partial\boldsymbol\lambda}{\partial\mathbf{aa}}\) is often neglected. The detailed form can be obtained by plugging in the following \[ \tau\frac{\partial\boldsymbol\lambda}{\partial\mathbf{a}} = \left[\mathbf{E}+\mathbf{G}\mathbf{H}^{-1}\mathbf{G}\right]^{-1}\mathbf{G}\mathbf{H}^{-1}\mathbf{A} \\ \]