## 4 Core Algorithm
### Estimated Energy Cost Function  
As discussed above, an estimated energy cost function \(E(P_{a},P_{b})\) calculates the estimated energy cost from \(P_{a}\) to \(P_{b}\). This function is used to find the feasible node set \(S_{feasible}\) and explorable node set \(S_{explorable}\). In this section, the equations for the cost function are derived from the robot positioning-controlled system's response. Also, tradeoffs between the energy cost and the settling time of moving are discussed. The energy of moving the camera between two locations from \(P_{initial}\) to \(P_{final}\), results from the energy cost of DC motors in 6-DoFs robot arm joints rotating from joint angles \(q_{initial}\) to \(q_{final}\). Therefore, \(E(P_{initial},P_{final})\) can be described as the sum of the time integration of rotational power in each joint:  
\[E(P_{initial},P_{final})=\sum_{i=1}^{6}\int_{t_{0}^{i}}^{t_{j}^{i}}V^{i}.I^{i} \cdot dt \tag{22}\]  
where \(V^{i}\) is the voltage, and \(I^{i}\) is the current in the DC motor's circuit of the \(i^{th}\) joint. \(t_{0}^{i}\) and \(t_{j}^{i}\) are the initial time and final time of the \(i^{th}\) joint moving from its initial angle \(q_{initial}^{i}\) to its final angle \(q_{final}^{i}\). Without detailed derivation, the dynamic model of 6-dof revolutionary robotic manipulators and DC motors can be expressed as [31]:\[\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}\]  
\[J_{m}=J_{a}+J_{g},\quad B=B_{m}+\frac{K_{b}K_{m}}{R} \tag{24}\]  
\[c_{i,j,k}=\frac{1}{2}(\frac{\delta d_{k,j}}{\delta q_{i}}+\frac{\delta d_{k,j}} {\delta q_{j}}-\frac{\delta d_{i,j}}{\delta q_{k}}),\ \Phi_{k}=\frac{\delta V}{\delta q_{k}} \tag{25}\]  
\[i,j,k\in(1,\ 2,\ 3,\ 4,\ 5,\ 6) \tag{26}\]  
where Equations (23) and (24) express the \(k^{th}\) joint dynamics equation. \(q_{i}/q_{j}/q_{k}\) is the joint revolute variable. \(J_{a}\) and \(J_{g}\) represent the moment of inertia of the motor and the gear of the model. \(r_{k}\) is the gear ratio at the \(k^{th}\) joint, and \(B_{m}\) is the damping effect of the gear. \(d_{i,j}\) represents the entry of the inertial matrix of the robot manipulator at the \(i^{th}\) row and \(i^{th}\) column. \(c_{i,j,k}\) are Christoffel symbols and for a fixed \(k\), with \(c_{i,j,k}=c_{j,i,k}\). \(\Phi_{k}\) is the derivative of potential energy \(V\) with respect to \(k^{th}\) joint variance. \(K_{m}\) is the torque constant in \(N-m/amp\), \(K_{b}\) is the back emf constant, and \(R\) is Armature Resistance. Take \(u^{i}\) as the actuator input to the dynamic system (23) measured at the \(i^{th}\) joint from a designed controller. Therefore, \(u^{i}\) equals the right side of Equation (23). That is:  
\[V^{i}=\frac{rR}{K_{m}}u^{i} \tag{27}\]  
In addition, without derivation, the expression of current in the Laplace domain [31] is:  
\[(Ls+R)I^{i}(s)=V^{i}(s)-\frac{K_{b}}{r}sq^{i}(s) \tag{28}\]  
\[\text{Then}:\ I^{i}(s)=\frac{1}{(Ls+R)}V^{i}(s)-\frac{K_{b}}{r}\frac{s}{(Ls+ R)}q^{i}(s) \tag{29}\]  
\[\text{Take the inverse Laplace}:\ I^{i}(t)=\frac{1}{L}e^{-\frac{R}{L}t}*V^{i}(t)- \frac{K_{b}}{r}(\frac{1}{L}-\frac{R}{L^{2}}e^{-\frac{R}{L}t})*q^{i}(t) \tag{30}\]  
where \(L\) is armature inductance, \(K_{b}\) is the back emf constant, and \(*\) is convolutional multiplication. Therefore, the instant current in the time domain \(I^{i}(t)\) is a function of the instant voltage \(V^{i}(t)\) and the instant angle \(\text{q}(t)\). Various controller designs, such as the PID controller [32] and Youla controller [33], of 6-DoF revolute robotic manipulators have been well developed in many papers. In this paper, we use a previous Youla controller design [33] with feedback linearization (Figure 9) as the position controller of the robot manipulator that holds the camera. Figure 9: The block diagram of feedback linearization Youla control design used for the joint control loop.