I'm trying to simulate a 2DOF planar pendulum with a regresor-passivity control, the thing is I've been having some issues with my simulation, I'm using Simulink, but in theory I know my control law should make the states of the system to converge, however in Simulation this is not happening on the contrary the position variqables diverge. If possible it will be really helpful if someone could guide me here, maybe I'm doing something wrong in simulation or I didnt defined something properly in the theory. This is the link to the simulation file:simulation file (the extension is mdl, should work with any version of matlab, I used R2020). also here is a paper where I found more info bout regressors: "https://link.springer.com/content/pdf/10.1007%2F978-3-642-41610-1_88-1.pdf" .Let me show the theory background first
Mechanical system
\begin{equation}\label{eq:sis2} \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+\mathbf{g}(\mathbf{q})=\boldsymbol{\tau} \end{equation}
$\mathbf{q}(t)\in \mathbb{R}^{2\times 1}$, $\mathbf{M}(\mathbf{q})\in \mathbb{R}^{2\times 2}$, $\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\in \mathbb{R}^{2\times 2}$, $\mathbf{g}(\mathbf{\dot{q}})\in \mathbb{R}^{2\times 1}$, $\boldsymbol{\tau} \in \mathbb{R}^{2\times 1}$.
Inertia Matrix: $\begin{equation*} \mathbf{M}(\mathbf{q})=\begin{bmatrix} m_{11}&m_{12}\\ m_{21}&m_{22}\\ \end{bmatrix} \end{equation*}$, Coriolis:$\begin{equation*} \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})=\begin{bmatrix} -2\phi\dot{q}_{2}&-\phi\dot{q}_{2}\\ \phi\dot{q}_{1}&0\\ \end{bmatrix} \end{equation*} $, gravity vector: $\begin{equation*} \mathbf{g}(\mathbf{q})=\begin{bmatrix} (h_{1}+h_{2})g&h_{2}g \end{bmatrix}^{T} \end{equation*}$.
\begin{align*} &m_{11}=m_{2}L_{1}^2+m_{2}l^2_{c2}+m_{1}l^2_{c1}+I_{1}+I_{2}+2m_{2}L_{1}l_{c2}\cos (q_{2})\\ &m_{12}=m_{21}=m_{2}l_{c2}^2+m_{2}L_{1}l_{c2}\cos (q_{2})+I_{2}\\ &m_{22}=m_{2}l_{c2}^2+I_{2}\\ &\phi=m_{2}L_{1}l_{c2}\sin q_{2}\\ &h_{1}=(m_{1}l_{c1}+m_{2}L_{1})\sin q_{1}\\ &h_{2}=m_{2}l_{c2}\sin (q_{1}+q_{2})\\ \end{align*}
Physical values:
I have previously tested this model with a Calculated torque control for trajectory tracking, it worked just fine. So, I have proven stability in the sense of Lyapunov using an error variable $\mathbf{S}$, such that $\mathbf{S}=\dot{\mathbf{q}}-\dot{\mathbf{q}}_{r}$ y $\dot{\mathbf{q}}_{r}=\dot{\mathbf{q}}_ {d}-\alpha \Delta \mathbf{q}$, thus $\mathbf{S}=\Delta \dot{\mathbf{q}}+\alpha \Delta \mathbf{q}$. $\mathbf{S}$ is the extended error variable which is expected to converge to $(0,0)$.
Let us define the parameterized regressor:
\begin{equation}\label{eq:sis4} \mathbf{Y}_{r}(\ddot{\mathbf{q}},\dot{\mathbf{q}_{r}},\mathbf{q})\mathbf{\theta}=\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}_{r}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}_{r}}+\mathbf{g}(\mathbf{q}) \end{equation}
\begin{equation}\label{eq:yr} \mathbf{Y}_{r}(\ddot{\mathbf{q}}_{r},\dot{\mathbf{q}}_{r},\mathbf{q})=\begin{bmatrix} \ddot{q}_{1r}&\ddot{q}_{1r}&\ddot{q}_{1r}+\ddot{q}_{2r}&y_{1r}&\ddot{q}_{1r}&\ddot{q}_{1r}+\ddot{q}_{2r}&sin(q_{1})&sin(q_{1})&sin(q_{1}+q_{2})\\ 0&0&\ddot{q}_{1r}+\ddot{q}_{2r}&y_{2r}&0&\ddot{q}_{1r}+\ddot{q}_{2r}&0&0&sin(q_{1}+q_{2}) \end{bmatrix} \end{equation}
where,
\begin{align*} y_{1r}&=2cos(q_{2})\ddot{q}_{1r}+cos(q_{2})\ddot{q_{2r}}-2sin(q_{2})\dot{q}_{2}\dot{q}_{1r}-sin(q_{2})\dot{q}_{2}\dot{q}_{2r}\\ y_{2r}&=cos(q_{2})\ddot{q}_{1r}+sin(q_{2})\dot{q}_{1}\dot{q}_{1r} \end{align*}
and the parameters of the system:
\begin{equation} \mathbf{\theta}=\begin{bmatrix} m_{1}l^{2}_{c1}\\ m_{2}L^{2}_{1}\\ m_{2}l^{2}_{c2}\\ m_{2}L_{1}l^{2}_{c2}\\ I_{1}\\ I_{2}\\ m_{1}l_{c1}g\\ m_{2}L_{1}g \\ m_{2}l_{c2}g \end{bmatrix} \end{equation}
I took this regressor from the book "Adaptive control of robot manipulators" by An-Chyau Huang but I made some corrections since the original had typos.
Let us now define our control law:
\begin{equation} \mathbf{\tau}=-\mathbf{K_{d}}\mathbf{S} + \mathbf{Y}_{r}\mathbf{\theta} \end{equation}
The closed-loop system:
\begin{equation}\label{eq:sis5} \mathbf{M}(\mathbf{q})(\ddot{\mathbf{q}}-\ddot{\mathbf{q}_{r}})+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})(\dot{\mathbf{q}}-\dot{\mathbf{q}_{r}})+\mathbf{g}(\mathbf{q})-\mathbf{g}(\mathbf{q})=\tau - \mathbf{Y}_{r}(\ddot{\mathbf{q}},\dot{\mathbf{q}_{r}},\mathbf{q})\mathbf{\theta} \end{equation}
Reducing the previous expression:
\begin{equation}\label{eq:sis6} \mathbf{M}(\mathbf{q})\dot{S}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})S+K_{d}S=0 \end{equation}
Stability in the sense of Lyapunov
Candidate function: $\begin{equation} V=\frac{1}{2}S^{T}M(q)S \end{equation}$
Conditions to be satisfied to guarantee asympotic convergence: a) $\dot{V} < 0$, $S\neq 0$, b) $\lim_{|S| \rightarrow\infty}V=\infty$.
Condition b) is satisfied by $S>0$ if it grows $V$ grows radially. For condition b) we have:
\begin{equation*} \dot{V}=\frac{1}{2}\{\dot{S}^{T}MS+S^{T}\dot{M}S+S^{T}M\dot{S}\} \end{equation*}
since $M$ is symmetric,
\begin{align*} \dot{V}=&\frac{1}{2}\{S^{T}\dot{M}S+2S^{T}M\dot{S}\} \end{align*}
substituting $M\dot{S}$,
\begin{align*} \dot{V}=&\frac{1}{2}\{S^{T}\dot{M}S+2S^{T}(-CS-K_{s}S)\}\\ =&\frac{1}{2}\{S^{T}\dot{M}S-2S^{T}CS-2S^{T}K_{d}S\}\\ =&S^{T}(\frac{1}{2}\dot{M}-C)S-S^{T}K_{d}S \end{align*}
since $\frac{1}{2}\dot{M}-C$ is anti-symmetric, then $S^{T}(\frac{1}{2}\dot{M}-C)S=0$, therefore
\begin{align*} \dot{V}=&-S^{T}K_{d}S\leq-K_{d}||S||^{2} \end{align*}
b) is satisfied.
Now, since I must simulate the "Real plant" I will make use of the control law $\mathbf{\tau}$, variable $\mathbf{S}$ and the parameterized regressor $\mathbf{Y}_{r}\mathbf{\theta}$. So, what I'm doing is to build $\mathbf{\dot{q}}_{r}$ (as shown before:$\dot{\mathbf{q}}_{r}=\dot{\mathbf{q}}_ {d}-\alpha \Delta \mathbf{q}$)from a desired trayectory $\mathbf{\dot{q}}_{d}$ (I chose sine and cosine) and the "actual angular velocity" $\mathbf{\dot{q}}$ so that I can generate $\mathbf{S}=\dot{\mathbf{q}}-\dot{\mathbf{q}}_{r}=\Delta \dot{\mathbf{q}}+\alpha \Delta \mathbf{q}$. In my simulation I'm just changig the contotrol input $\mathbf{\tau}=\mathbf(K)_{s}*\mathbf(S)+\mathbf(Y)_{r}\mathbf(\theta)$. In simulation I'm integrating the following expression:
\begin{equation}\label{eq:sis11} \ddot{\mathbf{q}}=\mathbf{M}^{-1}*\{\tau -\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})*\dot{\mathbf{q}}-\mathbf{g}(\mathbf{q})\} \end{equation}
Simulation information (Matlab/Simulink)
I'm using a fixed integration step (0.0000) with the ODE sover Runge/Kuta-4 in simulink.
Simulation scheme:
Desired trajectories:
Control block:\
Regressor block:\
Position Response:
As you can see the response there is no convergence in the response in opposition to the theory which says it should. To be honest I don't know what I'm doing wrong thats why I need your help guys if possible. Thanks in advance fopr any help.
