# Finite-Horizon Kinetic Energy Optimization of a Redundant Space Manipulator
## 2 Materials and Methods  
### Mathematical Formulation  
A kinematically redundant manipulator is a manipulator with more DOF than controlled end-effector variables. The tracking problem of a redundant manipulator is the problem of following a reference end-effector trajectory \(\mathbf{x}_{\textit{ref}}\), expressed as a function of time \(t\) in the Cartesian space. This requires to find the joint variables \(\mathbf{q}(t)\) such that:  
\[\mathbf{x}_{\textit{ref}}(t)=\mathbf{x}(\mathbf{q}(t)) \tag{1}\]in which \(x\) is the actual end-effector trajectory. The redundancy implies that the problem expressed by (1) may have an infinite number of solutions in the joints space. The method hereby presented is based on the general solution computed with the Moore-Penrose pseudoinverse:  
\[\dot{\mathbf{q}}=\mathbf{J}^{+}\dot{\mathbf{x}}+\big{(}\mathbf{I}-\mathbf{J}^{+}\mathbf{J}\big{)}\dot{ \mathbf{q}}_{0} \tag{2}\]  
in which \(\mathbf{J}\) is the manipulator Jacobian, \(\mathbf{J}^{+}=\big{(}\mathbf{J}^{\dagger}\mathbf{J}\big{)}^{-1}\mathbf{J}^{T}\) is its Moore-Penrose pseudoinverse, \(\dot{\mathbf{x}}\) is the desired end-effector velocity, \(\mathbf{I}\) is the identity matrix, and \(\dot{\mathbf{q}}_{0}\) is an arbitrary vector. The first part of (2) is a least-squares solution based on the minimization of the joint velocities norm and is related to the primary task (end-effector trajectory tracking), while the second part is related to a secondary task that can be imposed by exploiting the null-space operator [27]\(\big{(}\mathbf{I}-\mathbf{J}^{+}\mathbf{I}\big{)}\). The arbitrary vector \(\dot{\mathbf{q}}_{0}\) can be chosen so that it results in the minimization of an integral cost function along the specified end-effector trajectory, whereas the precision tracking is provided by the pseudoinverse solution (first term in (2)). A general expression of an integral cost function subject to the kinematic constraint related to the tracking of the desired end-effector trajectory is:  
\[ \begin{array}{ll}\underset{x}{\text{minimize}}&\int_{t_{0}}^{t_{fin}}C \big{(}\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}},t\big{)}dt\\ \text{\emph{subject to}}&\mathbf{x_{ref}}(t)=\mathbf{x}(\mathbf{q}(t))\end{array}  \tag{3}\]  
where \(C\big{(}\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}},t\big{)}\) is a cost function to be integrated along the manipulator trajectory, \(t_{0}\) and \(t_{fin}\) are the initial and final times. Final time \(t_{fin}\) does not need to be the time when the manipulator motion is completed: it is possible to define a time horizon \(h^{\prime}\), much shorter than the whole motion time, so that the integral cost is optimized in a time window from \(t_{0}\) to \(t_{0}+h^{\prime}\), and then updated during the motion (using each time an updated \(t_{0}\)), until \(t_{0}+h^{\prime}\) corresponds to \(t_{fin}\). This method is used here to optimize the kinetic energy integral of a serial manipulator, leading to the following discrete formulation:  
\[ \begin{array}{ll}\underset{\mathbf{q}}{\text{minimize}}&\sum\limits_{i=t_{0}}^ {t_{0}+h^{\prime}}\frac{1}{2}\dot{\mathbf{q}}_{i}^{T}\mathbf{M}_{i}(\mathbf{q}_{i})\dot{ \mathbf{q}}_{i}\Delta t\\ \text{\emph{subject to}}&\mathbf{x}(\mathbf{q}_{i})=\mathbf{x}_{i,ref}\qquad\quad for\;i=t_ {0}..t_{fin}\end{array}  \tag{4}\]  
where \(\mathbf{x}_{i,ref}\) is the reference end-effector position at time \(i\), \(\mathbf{x}(\mathbf{q}_{i})\) the actual end-effector position at time \(i\), \(\mathbf{q}_{i}\) the joints position vector at time \(i\), \(\mathbf{q}_{i}\) the joints velocity vector at time \(i\), \(\mathbf{M}_{i}(\mathbf{q}_{i})\) the inertia matrix at time \(i\), and \(\Delta t\) the discrete time step. For finite horizon optimization, as in the case presented here, \(t_{fin}\) in (3) is dropped in favour of a closer time \(t_{0}+h^{\prime}\), where \(h^{\prime}\) is a finite time horizon.