# Model-Based Real-Time Motion Tracking Using Dynamical Inverse Kinematics
## 5 Results
### Instantaneous Optimization  
As mentioned in Section 1, the instantaneous optimization methods solve the inverse kinematics at each time-step \(t_{k}\) through non-linear optimization. A general formulation of the optimization problem is defined as follows:  
\[\underset{q(t_{k})}{\text{minimize}} \left\|K_{r}r(q(t_{k}),x(t_{k}))\right\|_{2}\] (17a) subject to \[As(t_{k})\leq b \tag{17b}\]  
where \(K_{r}\) is a weight matrix that matches the unit measurements of target displacements, and eventually assigns a weight to each of the target. A common approach for solving the non-linear optimization problem is to consider the linear approximation of the system by recalling the Jacobian matrix definition, and solve the problem iteratively [48; 49]. However, in order to enforce state configuration constraints, recent approaches make also use of convex optimization [36; 50]. As benchmark, we have implemented instantaneous inverse kinematics optimization using iDynTree [51] multibody kinematics library, and the IPOPT software library for non-linear optimization [52]. The previous time-step solution has been used as warm-start for the non-linear optimizer, while the stopping criteria is the _pose error_ accuracy and it has been tuned in order to find a solution in a time comparable to the dynamical optimization. Two different implementations have been tested. The former, referred to as _whole-body optimization_, solves a single optimization problem instantiated for the whole-system. The latter, instead, instantiates the optimization process dividing the model into multiple subsystems, each consisting of exactly a pair of targets, and solves the sub-problems in parallel. We refer to this implementation as _pair-wise optimization_. An example of model splitting for _pair-wise optimization_ is shown in Figure 6.  
Looking at Figure 7, it can be observed that the performance of instantaneous optimization approaches decreases as the task gets more dynamic. This is particularly evident for the computational time. In the whole-body optimization, the average computational time is higher, and it is characterized by a large variance, reaching peaks above \(25\,\mathrm{ms}\) during the running task. Concerning the pair-wised optimization, the increase of time between walking and running is less evident. However, the pair-wised optimization takes longer for finding a solution for the iCub model because of the local difference between the human and the robot kinematics.  
Figure 6: A floating base model with eight links and four orientation targets (\({}^{\mathcal{I}}R_{B},{}^{\mathcal{I}}R_{C},{}^{\mathcal{I}}R_{\mathcal{D}},{} ^{\mathcal{I}}R_{E}\)) can be divided into three subsystems, with a pair of orientation targets each, in order to solve the inverse kinematics problem as _pair-wised_ instantaneous optimization.  
Figure 7: Comparison of the performance of inverse kinematics methods (_whole-body_, _pair-wised_, and _dynamical_) for three models (two humans, and iCub humanoid) in three different scenarios (_T-pose_, _Walking_, and _Running_). Each line contains the boxplots for a different performance evaluation metric, on the top the overall error for the orientation targets as base 10 logarithm of _mean normalized trace error_, in the middle line the overall error for the angular velocities as base 10 logarithm of _root mean squared error_, and at the bottom the computational time. Logarithmic metrics allows to compare metrics characterized by different order of magnitude in the different scenarios.