## 3 Modeling and Trajectory Planning
### Forward and Inverse Dynamics
, \tag{6}\]  
which is positive definite and symmetric, the vector of centrifugal, Coriolis and gyroscopic forces  
\[\mathbf{k}(\mathbf{q},\dot{\mathbf{q}})= \begin{bmatrix}-0.5\,\dot{\beta}^{2}l_{1}l_{2}m_{ 2}\sin(\beta)-\dot{\alpha}\dot{\beta}l_{1}l_{2}m_{2}\sin(\beta)\\ 0.5\,\dot{\alpha}^{2}l_{1}l_{2}m_{2}\sin(\beta)\end{bmatrix} , \tag{7}\]  
the vector of applied forces  
\[\mathbf{h}(\mathbf{q},\dot{\mathbf{q}})= \begin{bmatrix}0.5\,l_{1}m_{1}g\sin(\alpha)+m_{2} g\sin(\alpha+\beta)(0.5\,l_{2}+l_{1}cos(\beta))-l_{1}m_{2}g\cos(\alpha+\beta) \sin(\beta)-T_{\mathrm{fr},1}\\ 0.5\,l_{2}m_{2}g\sin(\alpha+\beta)-T_{\mathrm{fr},2}\end{bmatrix}  \tag{8}\]  
with the gravitational constant \(g\), and the input matrix  
\[\mathbf{D}= \begin{bmatrix}R_{1}k_{1}&0\\ 0&R_{2}k_{2}\end{bmatrix} . \tag{9}\]  
The method of inverse dynamics, or computed torque, is used to calculate a preliminary feed-forward controller for trajectory tracking. As proposed in Reference [30], this method is often most suitable for trajectory tracking of fully actuated MBS. Tracking the end-effector's desired trajectory in the Cartesian space is achieved by successively combining inverse dynamics with an inverse kinematics scheme. For this purpose, the corresponding trajectories of the generalized coordinates are tracked, which is known in robotics as tracking in the joint space or joint trajectory tracking [1]. Therefore, the desired trajectory of the end-effector \(\mathbf{r}_{\mathrm{ee,des}}(t)\), \(\mathbf{r}_{\mathrm{ee,des}}(t)\), and \(\mathbf{\dot{r}}_{\mathrm{ee,des}}(t)\) has to be transformed from the Cartesian space into the corresponding desired trajectory of the generalized coordinates \(\mathbf{q}_{\text{des}}(t)\), \(\hat{\mathbf{q}}_{\text{des}}(t)\), and \(\hat{\mathbf{q}}_{\text{des}}(t)\) in the joint space. This inverse problem can be stated formally as  
\[\mathbf{r}_{\text{ee,des}}=\mathbf{p}\left(\mathbf{q}_{\text{des}}\right)\quad\Rightarrow \quad\mathbf{q}_{\text{des}}=\mathbf{p}^{-1}\left(\mathbf{r}_{\text{ee,des}}\right), \tag{10}\]  
where \(\mathbf{p}^{-1}:\mathbb{R}^{3}\to\mathbb{R}^{n_{\text{DOF}}}\) is the inverse function of \(\mathbf{p}:\mathbb{R}^{n_{\text{DOF}}}\to\mathbb{R}^{3}\), which describes the forward kinematics of the end-effector as a function of the generalized coordinates. For the considered case holds  
\[\mathbf{r}_{\text{ee}}(\alpha,\beta)=\left[ \begin{array}{c}-l_{1}\sin(\alpha)-l _{2}\sin(\alpha+\beta)\\ l_{0}+l_{1}\cos(\alpha)+l_{2}\cos(\alpha+\beta)\\ 0\end{array} \right]. \tag{11}\]  
The solution of the inverse problem (10) is often not unique and multiple solutions might exist. In general, the nonlinear equations of the inverse kinematics problem can be solved using numerical techniques as for instance with a Newton-Raphson method [31], by differential kinematics, or in special cases algebraically if the manipulator has a particular topology. In the case of the considered two-link manipulator (even for the available manipulator with six DOF), an algebraic closed-form solution exists, see for example Reference [32]. Neglecting the end-effector's orientation and specifying only its position yields even in the case of a two-link planar arm two admissible postures, the so-called elbow-up and elbow-down configuration. Substituting the desired trajectory in terms of the generalized coordinates \(\mathbf{q}_{\text{des}}(t)\), \(\hat{\mathbf{q}}_{\text{des}}(t)\), and \(\hat{\mathbf{q}}_{\text{des}}(t)\) into the equations of motion (4) and solving the equations for the input vector yields  
\[\mathbf{u}_{\text{ff}}(t)=\mathbf{D}^{-1}\left[\mathbf{M}\left(\mathbf{q}_{\text{des}}(t) \right)\hat{\mathbf{q}}_{\text{des}}(t)+\mathbf{k}\left(\mathbf{q}_{\text{des}}(t),\hat{ \mathbf{q}}_{\text{des}}(t)\right)-\mathbf{h}\left(\mathbf{q}_{\text{des}}(t),\hat{\mathbf{q} }_{\text{des}}(t)\right)\right], \tag{12}\]  
where \(\mathbf{u}_{\text{ff}}(t)\) is the feed-forward controller.