## 2 Cooperative SCARA System
### Controllers
#### 2.4.1 Nonlinear Model Predictive Controller  
The Nonlinear Model Predictive Controller (NLMPC) uses a mathematical model of the system to predict the future evolution of its states over a prediction horizon. This approach allows for anticipation and correction of potential deviations before they occur, thereby improving control accuracy and efficiency. The NLMPC operates as follows:  
1. Prediction: The controller predicts how the states of the system will evolve in the future using a mathematical model. This includes considering the nonlinear dynamics of robots and their operating environments. 2. Optimization: The controller aims to minimize the cost function, which generally includes the trajectory tracking error and energy or actuation usage, penalizing excessive energy use or excessive control actions to prolong the lifespan of the system and improve efficiency. 3. Control Action Calculation: During sampling step, the controller solves an optimization problem to determine the optimal sequence of control actions. This problem can be complex because of the nonlinearity of the system and operational constraints. 4. Implementation and Update: Only the first action of the optimized sequence is implemented immediately, ensuring that the system is robust against disturbances and modeling errors. In the next sampling step, the problem is reformulated using the newly measured or estimated state of the system. This ensures that the controller continuously adapts to changing conditions and maintains a precise control. The optimization problem solved at each time step is formulated as follows:  
\[J(x(u),u)=\varnothing(x(N))+\sum_{K=0}^{N-1}L(x(k),u(k)) \tag{24}\]  
The instantaneous cost function \(L(x(k),u(k))\) is defined as  
\[L(x(k),u(k))=(x(\;k)-x_{ref}(k))^{T}Q(x(k)-x_{ref}(k))+u(k)^{T}Ru(k) \tag{25}\]  
and the terminal cost function \(\varnothing(x(N))\) is defined as  
\[\varnothing(x(N))=(x(N)\;\;-x_{ref}(N))^{T}P(x(N)-x_{ref}(N)) \tag{26}\]  
where \(x(k)\): System state at time step \(k\), with dimensions \(8\times 1\). \[x(k)=\left[q_{1}(k)\;q_{2}(k)\;q_{3}(k),q_{4}(k)\dot{q}_{1}(k)\dot{q}_{2}(k) \dot{q}_{3}(k)\dot{q}_{4}(k)\;\right]^{T} \tag{27}\]  
\(x_{ref}(k)\): Reference state vector or desired trajectory at time step \(k\), containing joint positions and velocities, with dimensions \(8\times 1\). \[x_{ref}(k)=\left[q_{ref,1}(k)q_{ref,2}(k)q_{ref,3}(k)q_{ref,4}(k)\dot{q}_{ref, 1}(k)\dot{q}_{ref,2}(k)\dot{q}_{ref,3}(k)\dot{q}_{ref,4}(k)\;\right]^{T} \tag{28}\]  
\(u(k)\): Control input vector at time step \(k\), with dimensions \(4\times 1\). \[u(k)= \begin{bmatrix}\tau_{1}(k)\\ \tau_{2}(k)\\ \tau_{3}(k)\\ \tau_{4}(k)\end{bmatrix}  \tag{29}\]  
Q: Positive definite weighting matrix that penalizes the tracking error, with dimensions \(8\times 8\). Each element \(Q_{1},\;Q_{2},\dots,\;Q_{8}\) represents the weight assigned to each component of the state vector \(x(k)\). \[Q=diag([Q_{1}\;Q_{2}\;Q_{3}\;Q_{4}\;Q_{5}\;Q_{6}\;Q_{7}\;Q_{8}]) \tag{30}\]  
R: Positive definite weighting matrix that penalizes the use of control inputs, with dimensions \(4\times 4\). \(r_{1},\;r_{2},\;r_{3},\;r_{4}\) are the weights assigned to each component of the control input vector \(u(k)\). \[R=diag([r_{1}\;r_{2}\;r_{3}\;r_{4}]) \tag{31}\]  
P: Positive definite weighting matrix that penalizes the tracking error at the final state, with dimensions \(8\times 8\). \[P=Q \tag{32}\]N: Prediction horizon, representing the number of future time steps that the controller considers when predicting the system's evolution and optimizing the sequence of control actions. The constraints for the optimization problem can be expressed as  
\[ \begin{array}{c}\mathrm{x}(0)=\ x_{\mathrm{current}}\\ \mathrm{x}(\mathrm{k}+1)=\mathrm{f}(\mathrm{x}(\mathrm{k}),\mathrm{u}(\mathrm{ k})),\ \mathrm{k}=0,\ \ldots,\ \mathrm{N}-1\\ \mathrm{g}(\mathrm{x}(\mathrm{k}),\mathrm{u}(\mathrm{k}))\leq 0,\ \mathrm{k}=0,\ \ldots,\ \mathrm{N}-1\\ \mathrm{h}(\mathrm{x}(\mathrm{k}),\mathrm{u}(\mathrm{k}))\leq 0,\ \mathrm{k}=0,\ \ldots,\ \mathrm{N}-1\end{array}