## 3 Modeling and Trajectory Planning
### Forward and Inverse Dynamics  
The schematic structure of the two-link manipulator and the inertial frame of reference \(\{\mathbf{e}_{1},\mathbf{e}_{2},\mathbf{e}_{3}\}\) used for modeling purpose are shown in Figure 5.  
The links are connected by revolute joints which represent the rotary units. The considered system has \(n_{\text{DOF}}=2\) degrees of freedom. Consequently, the kinematics of the two-link manipulator can be described by the generalized coordinates  
\[\mathbf{q}=[\alpha,\beta]^{\text{T}}, \tag{1}\]  
defined in Figure 5. The system parameters, that is, masses \(m_{1}\) and \(m_{2}\), inertias \(J_{1}\) and \(J_{2}\), link lengths \(l_{0}\), \(l_{1}\) and \(l_{2}\), and gear ratios \(R_{\text{PR90}}=R_{1}=R_{2}\) of the joints 1 and 2, are taken from CAD data and data sheets of the manufacturer. For the applied motor torques \(T_{1}\) and \(T_{2}\), a linear relation to the currents \(I_{1}\) and \(I_{2}\) is assumed yielding  
\[T_{i}=R_{i}\,k_{i}\,l_{i},\quad i=1,2, \tag{2}\]  
with the motor constants \(k_{i}\), which have been identified manually at the hardware. Following Reference [29], the friction torque in a rotary unit PR 90 with a Harmonic Drive gear can be stated as a function of the angular velocity \(\dot{q}_{i}\) in the respective unit \(i\) and the positive constants \(a_{j}\) as  
\[T_{\text{fr},i}=a_{1}\dot{q}_{i}+a_{2}\tanh(a_{3}\dot{q}_{i})+a_{4}\exp\left(- (\dot{q}_{i}/a_{5})^{2}\right)\tanh(3a_{3}\dot{q}_{i}). \tag{3}\]  
Figure 5: Schematic structure of the two-link manipulator illustrated by the multibody systems (MBS) software Neweul-M\({}^{2}\). To avoid the discontinuous \(sign\)-function to describe the share of Coulomb friction, the smooth \(tanh\)-function, that is, the term \(a_{2}\tanh(a_{3}\dot{q}_{i})\), is used instead. The share of viscous friction corresponds to the expression \(a_{1}\dot{q}_{i}\) and the superelevation of the stiction of the Stribeck curve is characterized by the last summand \(a_{4}\exp\left(-(\dot{q}_{i}/a_{5})^{2}\right)\tanh(3a_{3}\dot{q}_{i})\). The derivation of the equations of motion in minimal form is carried out with the Matlab-based research software Neweul-M\({}^{2}\)[3]. The forward dynamics is available for the whole six-joint PowerCube robot in Neweul-M\({}^{2}\) and therefore, the equations of motion of the whole system are available for further research. The computations are based on a Newton-Euler formalism with the application of the principle of D'Alembert [2]. Neweul-M\({}^{2}\) provides the equations of motion in a symbolical and a numerical fashion. In the latter case, Matlab's vast numerical abilities are used to perform time integrations, kinematic analysis, and animations of the results to check the behavior of the MBS. [PERSON]\({}^{2}\) uses the Matlab Symbolic Math Toolbox for the symbolical calculations. In modeling an MBS, this offers much flexibility since the symbolic expression of the equations of motion can be edited and extended at any time. Especially in control design, it is advantageous to have the equations of motion in symbolic form to apply advanced control strategies like the proposed MPC. The systematic modeling of the considered rigid MBS leads to the nonlinear equations of motion in minimal form  
\[\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{k}(\mathbf{q},\dot{\mathbf{q}})=\mathbf{h}(\mathbf{q},\dot{\mathbf{ q}})+\mathbf{D}\mathbf{u}, \tag{4}\]  
with the input vector  
\[\mathbf{u}=[I_{1},I_{2}]^{\mathrm{T}}, \tag{5}\]  
which consists of the motor currents of the first and second revolute joint, respectively. The dynamics in Equation (4) is characterized by the generalized mass matrix  
\[\mathbf{M}(\mathbf{q})= \begin{bmatrix}I_{1}+J_{2}+0.25(l_{1}^{2}m_{1}+l_{2}^{2}m_{2})+ l_{1}^{2}m_{2}+l_{1}l_{2}m_{2}\cos(\beta)&J_{2}+0.5\,l_{2}m_{2}(0.5\,l_{2}+l_{1} \cos(\beta))\\ J_{2}+0.5\,l_{2}m_{2}(0.5\,l_{2}+l_{1}\cos(\beta))&J_{2}+0.25\,l_{2}^{2}m_{2} \end{bmatrix}