## 4 Core Algorithm
### Estimated Energy Cost Function
From Figure 5, the actuator input \(u^{i}\) can be derived from the virtual input \(v^{i}\) so that:  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]0  
where M and H are nonlinear functions of \(q^{i}(t)\) and \(\overset{\cdot}{q^{i}(t)}\), the first derivative of \(q^{i}(t)\). Without showing the controller development in this paper, the 1-DoF controller transfer function \(G_{c}(s)\) and 1-DoF nominal plant \(G_{p}(s)\) with \(v^{i}\) as the virtual actuator input in the feedback linearization are expressed as follows:  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]1  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]2  
where \(\tau_{in}\) is a constant parameter in the controller design. Then, the following transfer functions can be calculated as:  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]3  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]4  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]5  
By taking inverse Laplace transform and \(q^{i}_{final}\) as step input, the following Equations (37)-(39) in the time domain are:  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]6  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]7  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]8  
With expressions of \(v^{i}(t)\), \(q^{i}(t)\), and \(\overset{\cdot}{q^{i}(t)}\), Equations (27) and (31) show that:  
\[\frac{1}{r_{k}}J_{mz}\ddot{q}_{k}+\sum_{j=1}^{6}d_{j,k}\ddot{q}_{j}+\sum_{i,j}^{6}c _{i,j,k}(q)\dot{q}_{i}\dot{q}_{j}+\frac{1}{r_{k}}\text{B}\dot{q}_{k}+\Phi_{k}(q)= \frac{K_{m}}{r_{k}R}V^{i} \tag{23}\]9  
and combine Equations (30) and (40):  
\[J_{m}=J_{a}+J_{g},\quad B=B_{m}+\frac{K_{b}K_{m}}{R} \tag{24}\]0  
where \(F\) and \(G\) are nonlinear functions of \(q^{i}_{initial}\), \(q^{i}_{final}\), and \(t\). It has been shown so far that the estimated energy cost \(E(P_{initial},P_{final})\) from location \(P_{initial}\) to \(P_{final}\) is a function of \(q_{initial}\) and \(q_{final}\). \(q_{initial}\) and \(q_{final}\) are derived from the inverse kinematics process [31] (shown in Appendix B, Equations (A6)-(A13)):  
\[q_{initial}=\text{{\it{\it{\it{\it{\it{\it{\it{\it{\it{\it{\it{\it{\it{ \it{\it{\it{\it{\it{\it{\it{\it{{\it  
If the delay of input is given or can be measured, the estimated energy cost can be calculated through the process in this section.