# Can we show converenge for a variant of inverse dynamics controller?

I was wondering if it is possible to show that a variant of the inverse dynamics controller

$$\tau = \mathbf{M}(q)\ddot{q}^\mathrm{des} + \mathbf{K}_pe + \mathbf{K}_d\dot{e} + \mathbf{h}(q, \dot{q})$$

is able to stabilize the system

$$\mathbf{M}(q)\ddot{q} + \mathbf{h}(q, \dot{q}) = \tau$$

The dynamics of the system is in the form of $$$$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) = \tau$$$$ and a variant of the inverse dynamics controller is in the form of $$$$\tau = \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}^\mathrm{des} + \mathbf{K}_p\Delta\mathbf{q} + \mathbf{K}_d\Delta\dot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}).$$$$ Inputting the control into the dynamics gives us \begin{align*} \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) &= \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}^\mathrm{des} + \mathbf{K}_p\Delta\mathbf{q} + \mathbf{K}_d\Delta\dot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}})\\ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} &= \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}^\mathrm{des} + \mathbf{K}_p\Delta\mathbf{q} + \mathbf{K}_d\Delta\dot{\mathbf{q}}\\ -\mathbf{M}(\mathbf{q})\Delta{\ddot{\mathbf{q}}} &= \mathbf{K}_p\Delta\mathbf{q} + \mathbf{K}_d\Delta\dot{\mathbf{q}} \end{align*} which can be written as $$$$\begin{bmatrix} \Delta\dot{\mathbf{q}}\\ \Delta\ddot{\mathbf{q}} \end{bmatrix} = \underbrace{\begin{bmatrix} 0 & \mathbf{I}\\ -\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_p & -\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_d \end{bmatrix}}_{\mathbf{A}(\mathbf{q})}\begin{bmatrix} \Delta\mathbf{q}\\ \Delta\dot{\mathbf{q}} \end{bmatrix}.$$$$ First, we show that $$\mathbf{A}(\mathbf{q})$$ is full rank, which leads to having non-zero eigenvalues. Using the property that elementary operations do not change the rank of a matrix, instead of investigating the rank of $$\mathbf{A}(\mathbf{q})$$, we look at the rank of $$$$\hat{\mathbf{A}}(\mathbf{q}) = \begin{bmatrix} \mathbf{I} & 0\\ -\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_d & -\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_p \end{bmatrix}$$$$ Then, by using Schur's complement, we have $$$$\det\Bigg(\begin{bmatrix} \mathbf{I} & 0\\ -\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_d & -\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_p \end{bmatrix}\Bigg) = \det(\mathbf{I})\det(-\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_p) = \det(-\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_p).$$$$ Using another property $$$$\mathrm{rank}(\mathbf{C}\mathbf{D}) \leq \min\{\mathrm{rank}(\mathbf{C}), \mathrm{rank}(\mathbf{D})\}$$$$ if we see $$\mathbf{D} = -\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_p$$ and $$\mathbf{C} = \mathbf{M}(\mathbf{q})$$, then we have $$$$\mathrm{rank}(-\mathbf{K}_p) \leq \min\{\mathrm{rank}(\mathbf{M}(\mathbf{q})), \mathrm{rank}(-\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_p)\}$$$$ which leads to the conclusion that $$-\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_p$$ is full rank with a non-zero determinant. Then, we have $$\hat{\mathbf{A}}(\mathbf{q})$$ have a non-zero determinant and full rank, which finally gives us the conclusion that $$\mathbf{A}(\mathbf{q})$$ is full rank. We then have the eigenvalues and eigenvectors of $$\mathbf{A}(\mathbf{q})$$ satisfying $$$$\begin{bmatrix} 0 & \mathbf{I}\\ -\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_p & -\mathbf{M}(\mathbf{q})^{-1}\mathbf{K}_d \end{bmatrix}\begin{bmatrix} \mathbf{v}_1\\ \mathbf{v}_2 \end{bmatrix} = \lambda\begin{bmatrix} \mathbf{v}_1\\ \mathbf{v}_2 \end{bmatrix}$$$$ with $$\lambda \neq 0$$, which gives us the relationship \begin{align} \mathbf{v}_2 &= \lambda\mathbf{v}_1\\ -\mathbf{M}(\mathbf{q})^{-1}(\mathbf{K}_p\mathbf{v}_1 + \mathbf{K}_d\mathbf{v}_2) &= \lambda\mathbf{v}_2 \end{align} This also tells us that $$\mathbf{v}_1$$ and $$\mathbf{v}_2$$ cannot be zero vectors. If one of them is the zero vector, then both of them will be zero, which gives a trivial solution to the equation above. Inputting the first equation into the second gives us $$$$-\mathbf{M}(\mathbf{q})^{-1}(\mathbf{K}_p + \lambda\mathbf{K}_d)\mathbf{v}_1 = \lambda^2\mathbf{v}_1$$$$ then, by multiplying $$\mathbf{M}(\mathbf{q})$$ on both sides, we have $$$$-\mathbf{K}_p\mathbf{v}_1 - \lambda\mathbf{K}_d\mathbf{v}_1 = \lambda^2\mathbf{M}(\mathbf{q})\mathbf{v}_1\quad\rightarrow\quad\lambda^2\mathbf{M}(\mathbf{q})\mathbf{v}_1 + \lambda\mathbf{K}_d\mathbf{v}_1 + \mathbf{K}_p\mathbf{v}_1 = 0$$$$ then, if we multiply $$\mathbf{v}_1^T$$ on the left, we have $$$$\label{eq:final_eq} (\mathbf{v}_1^T\mathbf{M}(\mathbf{q})\mathbf{v}_1)\lambda^2 + (\mathbf{v}_1^T\mathbf{K}_d\mathbf{v}_1)\lambda + \mathbf{v}_1^T\mathbf{K}_p\mathbf{v}_1 = 0.$$$$ We know that all of the coefficients are positive, then from the quadratic root formula $$$$\frac{-b\pm\sqrt{b^2-4ac}}{2a}$$$$ we can determine all of the roots of the above equation have negative real parts.

Thus, we can show that $$\mathbf{A}(\mathbf{q})$$ is a Hurwitz matrix, which means the system of $$\Delta{\mathbf{q}}$$ and $$\Delta{\dot{\mathbf{q}}}$$ is stable. This tells us the controller $$\tau$$ can stabilize the system to zero error.

• Nice work, is M(q) here the mass matrix in generalized coordinates? i.e. positive definite, symmetric? Commented Aug 12, 2022 at 18:23
• Yes, the $\mathbf{M}(\mathbf{q})$ matrix is positive definite and symmetric, here $\mathbf{q}$ is the generalized coordinates. Commented Aug 12, 2022 at 21:45

This would be too long for a comment, so I will add some work as an answer:

The proof of $$A(\mathbf{q})$$ being invertible is not perfect. First, add some structure to the $$K_p$$ and $$K_d$$ matrices. Assuming they are gain matrices, they should probably be positive definite and diagonal (thus invertible). In this case $$M(\mathbf{q})^{-1} K_p$$ and $$M(\mathbf{q})^{-1} K_d$$ are very easily shown to be full rank. $$\text{det}(AB) = \text{det}(A)\text{det}(B)$$. We also know that $$M(q)$$, $$K_p$$, and $$K_d$$ are all invertible and thus have nonzero determinant. To finalize the argument you could still use Schur's complement to show that $$A(\mathbf{q})$$ is invertible as $$\text{det}(I)\text{det}(-M(\mathbf{q})^{-1} K_p)$$ is nonzero.

To touch on adding structure to your variables before making arguments, there is technically no reason $$\mathbf{v}_{1}^{T}K_{d}\mathbf{v}_{1}$$ and $$\mathbf{v}_{1}^{T}K_{p}\mathbf{v}_{1}$$ would be guaranteed to be positive - unless you state it beforehand that $$K_d$$ and $$K_p$$ are positive definite! This may be well known to those familiar with PD control, but it is still good to explicitly mention this. Likewise, we do not know that $$-M(\mathbf{q})^{-1}K_{p}$$ is invertible as $$K_p$$ is not necessarily invertible unless some underlying assumptions are made.

As far as I can tell everything else looks good!