## 3 DCM with FMU
### The Substitution of RHS Equations with FMU  
This step aims to rebuild (22) with an FMU as a substitution of the RHS equations. Modelica is used here to build FMU; FMI offers supports the interface to use FMU as the RHS equations. Modelica is a language for physical modeling. The design approach builds on non-casual modeling with true equations and the use of object-oriented constructs to facilitate the reuse of modeling knowledge. As a result, it is convenient to write the dynamics equations in a freer form of which you do not have to guarantee there is one and only one variable on the left-hand side. With this property, the RHS equations can be derived implicitly by constructing the FMU of the robotic dynamics in Newton-Euler form. The robot dynamics model is constructed by a Modelica based software Mworks supporting drag-and-drop modeling [36]. As a result, the dynamics in the Newton-Euler form that have clear physical significance can be expressed easily in Mworks. The model is shown in Figure 2.  
Where the components tau1-tau6 represent the output torques of motors, the components inertia1-inertia6 represent the inertias of motors; the components r1-r6 represent the revolute joints in manipulators which shows the relationship of joint coordinates fixed on adjacent links; the components b1-b6 represent the link bodies in the manipulators which contain the iterative Newton-Euler dynamic formulation [37] of adjacent links. The outputs of components inertia 1-inertia6 are the output torques of gears, which are the input of components r1-r6. The dynamics in Newton-Euler form are constructed by operations of dragging-dropping function blocks. The components' revolute contains information of kinematic forward derivation in the Newton-Euler method, and the component link body contains physical properties of the robotic body and information of dynamic backward derivation in the Newton-Euler Method. The Modelica model can be transformed into FMU with FMI. The structure chart of FMU is shown in Figure 3.  
Figure 3: FMU structure chart. FMU is a black-box model used here to replace the analytic RHS equations; the input and output flows are presented in Figure 3. According to Figure 3, the FMU model, where the inputs are the present time \(t\), the present controls \(\mathbf{u}\), and the present states \(\mathbf{x}\), can derive the present differential states \(\dot{\mathbf{x}}\) by the internal solver. Therefore, it can be applied to the OCP solver as the RHS equations, and the difficulty in expressing the RHS equations is overcome. As a result, two black-box functions are used to express the dynamics of manipulators; i.e., Equations (7)-(9) are rewritten in the FMU form below:  
Figure 2: Modelica model of the manipulator. \[\tau=\text{FMU}_{inverse}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}},t), \tag{27}\] \[\ddot{\mathbf{q}}=\text{FMU}_{forward}(\tau,\mathbf{q},\dot{\mathbf{q}},t),\] (28) \[\mathbf{x}= \begin{pmatrix}\mathbf{q}\\ \dot{\mathbf{q}}\end{pmatrix} ,\dot{\mathbf{x}}_{\text{FMU}}= \begin{pmatrix}\dot\mathbf{{q}}\\ \text{FMU}_{forward}(\mathbf{\tau},\mathbf{q},\dot{\mathbf{q}},t)\end{pmatrix} . \tag{29}\]  
By discretizing (29), we have:  
\[ \begin{cases}\mathbf{X}_{i}=\begin{pmatrix}\mathbf{q}(T_{i})\\ \dot{\mathbf{q}}(T_{i})\end{pmatrix},\\ \dot{\mathbf{x}}_{\text{FMU},i}=\begin{pmatrix}\dot{\mathbf{q}}(T_{i})\\ \text{FMU}_{forward}(\tau(T_{i}),\mathbf{q}(T_{i}),\dot{\mathbf{q}}(T_{i}),T_{i}) \end{pmatrix}.\end{cases}  \tag{30}\]  
Then, the state equations are given by substituting (30) into (22); that is,  
\[\left\|\sum_{i=0}^{N}D_{ki}\mathbf{X}_{i}-\frac{t_{f}-t_{0}}{2}\dot{X}_{\text{FMU },i}\right\|_{\infty}\leq\delta,k=0,1,\ldots,N. \tag{31}\]  
All in all, the FMU model of manipulators, which is built by using the Newton-Euler method, can be applied in the DCM to determine the value of the RHS equations in (22).