# Simultaneous Floating-Base Estimation of Human Kinematics and Joint Torques

^{1}

^{2}

^{3}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Background

#### 2.1. Notation

- Let $\mathbb{R}$ and $\mathbb{N}$ be the set of real and natural numbers, respectively.
- Let $\mathit{x}\in {\mathbb{R}}^{n}$ denote a n-dimensional column vector, while x denotes a scalar quantity. We advise the reader to pay attention to the notation style: we define vectors, matrices with bold small and capital letters, respectively, and scalars with non-bold style.
- Let $\left|\mathit{x}\right|$ be the norm of the vector $\mathit{x}$.
- Let ${\mathbf{0}}_{m}$ and ${\mathbf{1}}_{m}$ be the zero and identity matrices $\in {\mathbb{R}}^{m\times m}$, respectively. The notation ${\mathbf{0}}_{m\times n}$ represents the zero matrix $\in {\mathbb{R}}^{m\times n}$.
- Let $\mathcal{I}$ be an inertial frame with z-axis pointing against the gravity (g denotes the norm of the gravitational acceleration). Let $\mathcal{B}$ denote the base frame, i.e., a frame attached to the base link. Let $\mathcal{L}$ be the generic frame attached to a link, and $\mathcal{J}$ the frame of a joint.
- Let each frame be identified by an origin and an orientation, e.g., $\mathcal{I}=({O}_{\mathcal{I}},\left[\mathcal{I}\right])$ or $\mathcal{L}\left[\mathcal{I}\right]=({O}_{\mathcal{L}},\left[\mathcal{I}\right])\phantom{\rule{3.33333pt}{0ex}}$.
- Let ${}^{\mathcal{I}}{\mathit{o}}_{\mathcal{B}}\in {\mathbb{R}}^{3}$ be the coordinate vector connecting ${O}_{\mathcal{I}}$ with ${O}_{\mathcal{B}}$, pointing towards ${O}_{\mathcal{B}}$, expressed with respect to (w.r.t.) frame $\mathcal{I}$.
- Let ${}^{\mathcal{I}}{\mathit{R}}_{\mathcal{B}}\in SO\left(3\right)$ be a rotation matrix such that ${}^{\mathcal{I}}{\mathit{o}}_{\mathcal{L}}={}^{\mathcal{I}}{\mathit{o}}_{\mathcal{B}}+{}^{\mathcal{I}}{\mathit{R}}_{\mathcal{B}}\phantom{\rule{3.33333pt}{0ex}}{}^{\mathcal{B}}{\mathit{o}}_{\mathcal{L}}\phantom{\rule{3.33333pt}{0ex}}$.
- Let $\mathbf{S}\left(\mathit{x}\right)\in SO\left(3\right)$ denote the skew-symmetric matrix such that $\mathbf{S}\left(\mathit{x}\right)\mathit{y}=\mathit{x}\times \mathit{y}$, being × the cross product operator in ${\mathbb{R}}^{3}$.
- Let ${}^{\mathcal{I}}{\dot{\mathit{o}}}_{\mathcal{B}}$ and ${}^{\mathcal{I}}{\ddot{\mathit{o}}}_{\mathcal{B}}$ denote the first-order and second-order time derivatives of ${}^{\mathcal{I}}{\mathit{o}}_{\mathcal{B}}\phantom{\rule{3.33333pt}{0ex}}$, respectively.
- Given a stochastic variable $\mathit{x}$, let $p\left(\mathit{x}\right)$ denote its probability density and $p\left(\mathit{x}\right|\mathit{y})$ the conditional probability of $\mathit{x}$ given the assumption that another stochastic variable $\mathit{y}$ has occurred.
- If $E\left[\mathit{x}\right]$ is the expected value of a stochastic variable $\mathit{x}$, let ${\mathit{\mu}}_{\mathit{x}}=E\left[\mathit{x}\right]$ and ${\mathbf{\Sigma}}_{\mathit{x}}=E\left[\mathit{x}{\mathit{x}}^{\top}\right]$ be the mean and covariance of $\mathit{x}$, respectively. Let $\mathit{x}\sim \mathcal{N}({\mathit{\mu}}_{\mathit{x}},{\mathbf{\Sigma}}_{\mathit{x}})$ be the expression for the normal distribution of $\mathit{x}$.

#### 2.2. Human Kinematics and Dynamics Modeling

#### 2.3. Case-Study Human Model

## 3. Simultaneous Floating-Base Estimation of Human Whole-Body Kinematics and Dynamics

#### 3.1. Offline Estimation of Sensor Position

#### 3.2. Estimation of Human Kinematics

#### 3.3. Offline Contact Classification

Algorithm 1 Offline Feet Contact Cassification. | |

Require: FT sensor forces (z component) for right foot ${}^{\mathcal{R}F}{\mathit{f}}_{z}$ and left foot ${}^{\mathcal{L}F}{\mathit{f}}_{z}$ | |

1: | procedure |

2: | N ← number of samples |

3: | ${T}_{{f}_{z}}$ ← threshold on f_{z} = mean(${}^{\mathcal{R}F}{\mathit{f}}_{z}+{}^{\mathcal{L}F}{\mathit{f}}_{z}$) |

4: | main loop: |

5: | for $j=1\to N\phantom{\rule{3.33333pt}{0ex}}$ do |

6: | if $abs\left(\right)open="("\; close=")">{}^{\mathcal{R}F}{\mathit{f}}_{z}-{}^{\mathcal{L}F}{\mathit{f}}_{z}$ then |

7: | Classify j as double support sample |

8: | else |

9: | if ${}^{\mathcal{R}F}{\mathit{f}}_{z}\left(j\right)>{}^{\mathcal{L}F}{\mathit{f}}_{z}\left(j\right)$ then |

10: | Classify j as right single support sample |

11: | else |

12: | Classify j as left single support sample |

13: | end if |

14: | end if |

15: | end for |

16: | end procedure |

#### 3.4. Maximum-A-Posteriori Algorithm for Floating-Base Dynamics Estimation

- We used the new acceleration representation by exploiting the relation between Equation (11) and Equation (12), i.e.,$${\mathit{a}}_{\mathcal{L}}^{g}={\mathbf{\alpha}}_{\mathcal{L}}^{g}-{\overline{\mathbf{\alpha}}}_{\mathcal{L}}^{g}\text{\hspace{1em}\hspace{1em}\hspace{1em}}\mathrm{being}\text{\hspace{1em}\hspace{1em}\hspace{1em}}{\overline{\mathbf{\alpha}}}_{\mathcal{L}}^{g}=\left[\begin{array}{c}\left({}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}{}^{\mathcal{I}}{\dot{\mathit{o}}}_{\mathcal{L}}\right)\times {\mathbf{\omega}}_{\mathcal{L}}\\ {\mathbf{0}}_{3\times 1}\end{array}\right]$$
- Since we broke the univocal relation between each link and its parent joint, we redefine the serialization of all the kinematics and dynamics quantities in the vector $\mathit{d}$ w.r.t. the fixed-base serialization of the same vector in Section 4 of [7], thus$$\begin{array}{ccc}\hfill \mathit{d}& =& {\left[\begin{array}{cc}{\mathit{d}}_{link}^{\top}& {\mathit{d}}_{joint}^{\top}\end{array}\right]}^{\top}\in {\mathbb{R}}^{12{N}_{B}+7n}\phantom{\rule{3.33333pt}{0ex}},\hfill \end{array}$$$$\begin{array}{ccc}\hfill {\mathit{d}}_{link}& =& \left[\begin{array}{ccccccc}{\mathbf{\alpha}}_{0}^{g}& {\mathrm{\mathbb{f}}}_{0}^{x}& {\mathbf{\alpha}}_{1}^{g}& {\mathrm{\mathbb{f}}}_{1}^{x}& \cdots & {\mathbf{\alpha}}_{{N}_{B}-1}^{g}& {\mathrm{\mathbb{f}}}_{{N}_{B}-1}^{x}\end{array}\right]\in {\mathbb{R}}^{12{N}_{B}}\phantom{\rule{3.33333pt}{0ex}},\hfill \end{array}$$$$\begin{array}{ccc}\hfill {\mathit{d}}_{joint}& =& \left[\begin{array}{cccccccc}{\mathrm{\mathbb{f}}}_{1}& {\mathrm{\mathbb{f}}}_{2}& \cdots & {\mathrm{\mathbb{f}}}_{n}& {\ddot{s}}_{1}& {\ddot{s}}_{2}& \cdots & {\ddot{s}}_{n}\end{array}\right]\in {\mathbb{R}}^{7n}\phantom{\rule{3.33333pt}{0ex}}.\hfill \end{array}$$
- The variable $\mathit{\tau}$ was removed from $\mathit{d}$. The joint torque can be obtained as a projection of the joint wrench on the motion freedom subspace, such that ${\tau}_{\mathrm{J}}={\mathbb{S}}_{\mathrm{J}}^{\top}{\mathrm{\mathbb{f}}}_{\mathrm{J}}$, for each joint $\mathrm{J}$ of the model.

- The first set of equations $\mathit{Y}\left(\mathit{s}\right)\mathit{d}+{\mathit{b}}_{Y}(\mathit{s},\dot{\mathit{s}})=\mathit{y}\phantom{\rule{3.33333pt}{0ex}}$ accounts for the sensor measurements. The number of equations depends on how many sensors are conveyed into the vector $\mathit{y}\in {\mathbb{R}}^{y}$ and it does not depend on the number of links in the model (more than one sensor could be associated to the same link, e.g., the combination of an IMU + a FT sensor). In general, the sensor matrices are not changed within the new floating-base formalism. The only difference is that the accelerometer has a different relation with the acceleration of the body. In particular, if the frame $\mathcal{L}$ of a link and the frame associated to the IMU located on the same link are rigidly connected, then$$\begin{array}{c}\hfill {\mathit{y}}_{\mathcal{L},IMU}={\mathbf{\alpha}}_{IMU}^{g}={}^{\mathcal{L}}{\mathit{X}}_{\mathcal{B}}{\mathbf{\alpha}}_{\mathcal{B}}^{g}+\left[\begin{array}{c}{}^{\mathcal{L}}{\mathit{R}}_{\mathcal{B}}\left(\right)open="["\; close="]">{\mathbf{\omega}}_{\mathcal{B}}\times \left(\right)open="("\; close=")">{\mathbf{\omega}}_{\mathcal{B}}\times {}^{\mathcal{B}}{\mathit{o}}_{IMU}\end{array}\right]{\mathbf{0}}_{3\times 1}& .\end{array}$$$$\begin{array}{c}\hfill {\mathit{y}}_{\mathcal{L},FT}={}^{\mathcal{L}}{\mathit{X}}_{FT}^{*}{\mathrm{\mathbb{f}}}_{FT}\phantom{\rule{3.33333pt}{0ex}}.\end{array}$$
- The second set of equations $\mathit{D}\left(\mathit{s}\right)\mathit{d}+{\mathit{b}}_{D}(\mathit{s},\dot{\mathit{s}})=\mathbf{0}\phantom{\rule{3.33333pt}{0ex}}$ represents the compact matrix form for Equations (16) and (17) given the new serialization of $\mathit{d}$ in Equation (14). The matrix $\mathit{D}\in {\mathbb{R}}^{12{N}_{B}\times d}$ is a matrix with $12{N}_{B}$ rows and d columns, i.e., the number of rows of $\mathit{d}$ in Equation (14). The matrix blocks in $\mathit{D}$ for the acceleration of Equation (16) are recursively the following:$$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathbf{\alpha}}_{\mathrm{L}}}& =& -{\mathbf{1}}_{6}\phantom{\rule{3.33333pt}{0ex}},\hfill \end{array}$$$$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathbf{\alpha}}_{\lambda \left(\mathrm{L}\right)}}& =& \left(\right)open="\{"\; close>\begin{array}{cc}{}^{\mathrm{L}}{\mathit{X}}_{\lambda \left(\mathrm{L}\right)}\in {\mathbb{R}}^{6\times 6}\hfill & \forall \mathrm{L}\ne \mathcal{B}\hfill \\ {\mathbf{0}}_{6}\hfill & otherwise\hfill \end{array}\hfill \end{array}$$$$\begin{array}{ccc}\hfill {\mathit{D}}_{{\ddot{\mathit{s}}}_{\mathrm{J}}}& =& \left(\right)open="\{"\; close>\begin{array}{cc}{\mathbb{S}}_{\mathrm{J}}\in {\mathbb{R}}^{6}\hfill & \forall \mathrm{L}\ne \mathcal{B}\hfill \\ {\mathbf{0}}_{6\times 1}\hfill & otherwise\hfill \end{array}\hfill \end{array}$$$$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathbf{\alpha}}_{\mathrm{L}}}& =& {\mathit{M}}_{\mathrm{L}}\in {\mathbb{R}}^{6\times 6}\phantom{\rule{3.33333pt}{0ex}},\hfill \end{array}$$$$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathit{f}}_{\mathrm{L}}^{x}}& =& -{\mathbf{1}}_{6}\phantom{\rule{3.33333pt}{0ex}},\hfill \end{array}$$$$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathit{f}}_{\mathrm{J}}}& =& \left(\right)open="\{"\; close>\begin{array}{cc}-{\mathbf{1}}_{6}\hfill & \forall \mathrm{L}\ne \mathcal{B}\hfill \\ {\mathbf{0}}_{6}\hfill & otherwise\hfill \end{array}\hfill \end{array}$$$$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathit{f}}_{{\mathrm{J}}_{\mu}}}& =& \left(\right)open="\{"\; close>\begin{array}{cc}{}^{\mathrm{J}}{\mathit{X}}_{{\mathrm{J}}_{\mu}}^{*}\in {\mathbb{R}}^{6\times 6}\hfill & if\phantom{\rule{3.33333pt}{0ex}}\exists \phantom{\rule{3.33333pt}{0ex}}\mu \left(\mathrm{L}\right)\hfill \\ {\mathbf{0}}_{6}\hfill & otherwise\hfill \end{array}\hfill \end{array}$$
**0**. Unlike $\mathit{D}$, the term ${\mathit{b}}_{D}\in {\mathbb{R}}^{12{N}_{B}}$ is affected by the new representation of the acceleration w.r.t. the one in [7]. Each subterm ${\mathit{b}}_{{D}_{L}}\in {\mathbb{R}}^{12}$ is such that$${\mathit{b}}_{{D}_{L}}=\{\begin{array}{c}\left(\right)open="["\; close="]">\begin{array}{c}\phantom{\rule{0.0pt}{0ex}}{\overline{\mathbf{\alpha}}}_{\mathcal{B}}^{g}\\ \left[\begin{array}{c}{\mathbf{0}}_{3\times 1}\\ {\mathbf{\omega}}_{\mathcal{B}}\end{array}\right]{\times}^{*}{\mathit{M}}_{\mathcal{B}}\left[\begin{array}{c}{\mathbf{0}}_{3\times 1}\\ {\mathbf{\omega}}_{\mathcal{B}}\end{array}\right]\hfill \end{array}\\ if\hspace{1em}\mathrm{L}=\mathcal{B}\end{array}\left(\right)open="["\; close="]">\begin{array}{c}\left[\begin{array}{c}\left[\begin{array}{c}{\mathbf{0}}_{3\times 1}\\ {\mathbf{\omega}}_{\mathrm{L}}\end{array}\right]\times {\mathbb{S}}_{\mathrm{J}}{\dot{\mathit{s}}}_{\mathrm{J}}+\left(\right)open="("\; close>{\overline{\mathbf{\alpha}}}_{\mathrm{L}}^{g}-{}^{\mathrm{L}}{\mathit{X}}_{\lambda \left(\mathrm{L}\right)}{}^{\lambda \left(\mathrm{L}\right)}\mathbf{\alpha}\end{array}\right]\phantom{\rule{0.0pt}{0ex}}{\overline{\lambda}}_{\left(\mathrm{L}\right)}^{g}\end{array}\left[\begin{array}{c}{\mathbf{0}}_{3\times 1}\\ {\mathbf{\omega}}_{\mathrm{L}}\end{array}\right]{\times}^{*}{\mathit{M}}_{\mathrm{L}}\left[\begin{array}{c}{\mathbf{0}}_{3\times 1}\\ {\mathbf{\omega}}_{\mathrm{L}}\end{array}\right]$$

**d**and

**y**are stochastic variables with Gaussian distributions and the problem is solved by maximizing the conditional probability of $\mathit{d}$ given the measurements $\mathit{y}$, i.e.,

## 4. Experiments and Analysis

#### 4.1. Experimental Setup

^{®}. The MAP computation code (open-source at https://github.com/claudia-lat/MAPest) relies on the C++ based iDynTree multi-body dynamics library designed for free floating robots [26]. iDynTree is released as open source code available on Github: https://github.com/robotology/idyntree. Furthermore, it is worth remarking that an important modification on the IK computation was here introduced w.r.t. the one in [7]. We removed the OpenSim IK toolbox dependency and we computed the whole-body joint angles with an Ipopt-based IK (see Section 3.2).

#### 4.2. Comparison between Measurement and Estimation

**d**in Equation (14) contains variables measurable via sensors (${\mathit{\alpha}}_{lin}^{g}$, ${\mathit{f}}^{x}$ and ${\mathit{m}}^{x}$) and variables that cannot be measured in humans ($\mathrm{\mathbb{f}}$ and $\mathit{\tau}$) but only estimated with the algorithm. The MAP algorithm represents the probabilistic way to estimate those quantities for which a quantitative measure does not exist. The objective is to compare the same variables (measured and estimated) to prove the goodness of the proposed algorithm. Figure 7 shows the comparison for the base linear proper sensor acceleration ${\mathit{\alpha}}_{lin}^{g}$ [$\mathrm{m}/{\mathrm{s}}^{2}$] between the measurement (mean and standard deviation, in red) and the estimation via algorithm (mean, in blue), for tasks T1, T2, T3 and T4, respectively. The same comparison for the external force ${\mathit{f}}^{x}$[N] and external moment ${\mathit{m}}^{x}$ [Nm] is shown in Figure 8a,b for the left foot and right foot, respectively. The validation was performed along with a Root Mean Square Error (RMSE) investigation for linear accelerations and external wrenches (Table 2). Error range values are shown in Table 3.

#### 4.3. Human Joint Torques Estimation during Gait

#### 4.4. Comparison between Fixed-Base and Floating-Base Algorithms

#### 4.5. A Word of Caution on the Covariances Choice

- low values for the covariance ${\mathbf{\Sigma}}_{y}$ if trusting in the sensor measurements;
- low values for the model covariance ${\mathbf{\Sigma}}_{D}$ for trusting the dynamic model; and
- high values for the covariance ${\mathbf{\Sigma}}_{d}$, which means that the end-user does not know any a priori information on the estimation.

## 5. Conclusions

## Author Contributions

## Funding

## Conflicts of Interest

## Appendix A

**d**and

**y**be stochastic variables with Gaussian distributions. The conditional probability of

**d**given

**y**is

- The conditional probability $p\left(\mathit{y}\right|\mathit{d})$ is:$$\begin{array}{ccc}\hfill p\left(\mathit{y}\right|\mathit{d})& \propto & exp-\frac{1}{2}\left(\right)open="\{"\; close="\}">{\left(\right)}^{\mathit{y}}\top {\mathbf{\Sigma}}_{y|d}^{-1}\left(\right)open="("\; close=")">\mathit{y}-{\mathbf{\mu}}_{y|d}\hfill \end{array}$$
- Let $\mathit{d}\sim \mathcal{N}({\overline{\mathit{\mu}}}_{D},{\overline{\mathbf{\Sigma}}}_{D})$ the normal distribution of
**d**. The probability density p(**d**)$$p\left(\mathit{d}\right)\propto exp-\frac{1}{2}\left(\right)open="\{"\; close="\}">{(\mathit{d}-{\overline{\mathit{\mu}}}_{D})}^{\top}{\overline{\mathbf{\Sigma}}}_{D}^{-1}(\mathit{d}-\mathit{\mu}{\overline{\mu}}_{D})$$$${\overline{\mathbf{\Sigma}}}_{D}={({D}^{\top}{\mathbf{\Sigma}}_{D}^{-1}D+{\mathbf{\Sigma}}_{d}^{-1})}^{-1}$$$${\overline{\mu}}_{D}={\overline{\mathbf{\Sigma}}}_{D}({\mathbf{\Sigma}}_{d}^{-1}{\mu}_{D}-{D}^{\top}{\mathbf{\Sigma}}_{D}^{-1}{\mathit{b}}^{D})$$ - To compute $p\left(\mathit{d}\right|\mathit{y})$, it suffices to combine Equations (A3) and (A4),$$\begin{array}{c}\hfill p\left(\mathit{d}\right|\mathit{y})\propto exp-\frac{1}{2}\left(\right)open="\{"\; close="\}">{(\mathit{d}-{\overline{\mathbf{\mu}}}_{D})}^{\top}{\overline{\mathbf{\Sigma}}}_{D}^{-1}(\mathit{d}-{\overline{\mathbf{\mu}}}_{D})+{[y-(\mathit{Y}\mathit{d}+{\mathit{b}}_{\mathit{Y}}\left)\right]}^{\top}{\overline{\mathbf{\Sigma}}}_{y|d}^{-1}[y-(\mathit{Y}\mathit{d}+{\mathit{b}}_{\mathit{Y}}\left)\right]\end{array}$$$$\begin{array}{ccc}\hfill {\mathbf{\Sigma}}_{d|y}& =& {\overline{\mathbf{\Sigma}}}_{D}^{-1}+{\mathit{Y}}^{\top}{\mathbf{\Sigma}}_{y|d}^{-1}{\mathit{Y}}^{-1},\hfill \end{array}$$$${\mathbf{\mu}}_{d|y}={\mathbf{\Sigma}}_{d|y}\left(\right)open="["\; close="]">{\mathit{Y}}^{\top}{\mathbf{\Sigma}}_{y|d}^{-1}(\mathit{y}-{\mathit{b}}_{Y})+{\overline{\mathbf{\Sigma}}}_{D}^{-1}{\overline{\mathbf{\mu}}}_{D}$$In the Gaussian domain, the MAP solution coincides with the mean in Equation (A7b) yielding to:$$\begin{array}{c}\hfill {\mathit{d}}_{MAP}={\mathbf{\mu}}_{d|y}.\end{array}$$

## References

- Tirupachuri, Y.; Nava, G.; Latella, C.; Ferigo, D.; Rapetti, L.; Tagliapietra, L.; Nori, F.; Pucci, D. Towards Partner-Aware Humanoid Robot Control Under Physical Interactions. arXiv
**2019**, arXiv:1809.06165. [Google Scholar] - Flash, T.; Hogan, N. The coordination of arm movements: An experimentally confirmed mathematical model. J. Neurosci.
**1985**, 5, 1688–1703. [Google Scholar] [CrossRef] [PubMed] - Maeda, Y.; Hara, T.; Arai, T. Human-robot cooperative manipulation with motion estimation. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180), Maui, HI, USA, 29 October–3 November 2001; Volume 4, pp. 2240–2245. [Google Scholar] [CrossRef]
- Schaal, S.; Ijspeert, A.; Billard, A. Computational approaches to motor learning by imitation. Philosoph. Trans. R. Soc. Lond. Ser. B Biol. Sci.
**2003**, 358, 537–547. [Google Scholar] [CrossRef] [PubMed][Green Version] - Amor, H.B.; Neumann, G.; Kamthe, S.; Kroemer, O.; Peters, J. Interaction primitives for human-robot cooperation tasks. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, 31 May–7 June 2014; pp. 2831–2837. [Google Scholar] [CrossRef]
- Penco, L.; Brice, C.; Modugno, V.; Mingo Hoffmann, E.; Nava, G.; Pucci, D.; Tsagarakis, N.; Mouret, J.B.; Ivaldi, S. Robust Real-time Whole-Body Motion Retargeting from Human to Humanoid. In Proceedings of the IEEE-RAS 18th International Conference on Humanoid Robots (Humanoids), Beijing, China, 6–9 November 2018. [Google Scholar]
- Latella, C.; Lorenzini, M.; Lazzaroni, M.; Romano, F.; Traversaro, S.; Akhras, M.A.; Pucci, D.; Nori, F. Towards real-time whole-body human dynamics estimation through probabilistic sensor fusion algorithms. Auton. Robots
**2018**. [Google Scholar] [CrossRef] - Mistry, M.; Buchli, J.; Schaal, S. Inverse dynamics control of floating base systems using orthogonal decomposition. In Proceedings of the IEEE International Conference on Robotics and Automation, Anchorage, AK, USA, 3–7 May 2010; pp. 3406–3412. [Google Scholar] [CrossRef]
- Nava, G.; Pucci, D.; Guedelha, N.; Traversaro, S.; Romano, F.; Dafarra, S.; Nori, F. Modeling and Control of Humanoid Robots in Dynamic Environments: iCub Balancing on a Seesaw. In Proceedings of the IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids), Birmingham, UK, 15–17 November 2017. [Google Scholar]
- Ayusawa, K.; Venture, G.; Nakamura, Y. Identification of humanoid robots dynamics using floating-base motion dynamics. In Proceedings of the 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, Nice, France, 22–26 September 2008; pp. 2854–2859. [Google Scholar] [CrossRef]
- Mistry, M.; Schaal, S.; Yamane, K. Inertial parameter estimation of floating base humanoid systems using partial force sensing. In Proceedings of the 2009 9th IEEE-RAS International Conference on Humanoid Robots, Paris, France, 7–10 December 2009; pp. 492–497. [Google Scholar] [CrossRef]
- Dasgupta, A.; Nakamura, Y. Making feasible walking motion of humanoid robots from human motion capture data. In Proceedings of the 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C), Detroit, MI, USA, 10–15 May 1999; Volume 2, pp. 1044–1049. [Google Scholar] [CrossRef]
- Zheng, Y.; Yamane, K. Human motion tracking control with strict contact force constraints for floating-base humanoid robots. In Proceedings of the 2013 13th IEEE-RAS International Conference on Humanoid Robots (Humanoids), Atlanta, GA, USA, 15–17 October 2013; pp. 34–41. [Google Scholar] [CrossRef]
- Li, M.; Deng, J.; Zha, F.; Qiu, S.; Wang, X.; Chen, F. Towards Online Estimation of Human Joint Muscular Torque with a Lower Limb Exoskeleton Robot. Appl. Sci.
**2018**, 8, 1610. [Google Scholar] [CrossRef] - Marsden, J.E.; Ratiu, T. Introduction to Mechanics and Symmetry: A Basic Exposition of Classical Mechanical Systems, 2nd ed.; Texts in Applied Mathematics; Springer-Verlag: New York, NY, USA, 1999. [Google Scholar] [CrossRef]
- Winter, D. Biomechanics and Motor Control of Human Movement, 4th ed.; Wiley: Hoboken, NJ, USA, 1990. [Google Scholar]
- Herman, I. Physics of the Human Body | Irving P. Herman |; Springer: Basel, Switzerland, 2016. [Google Scholar] [CrossRef]
- Hanavan, E.P. A Mathematical Model of the Human Body. Available online: https://apps.dtic.mil/docs/citations/AD0608463 (accessed on 19 June 2019).
- Yeadon, M.R. The simulation of aerial movement—II. A mathematical inertia model of the human body. J. Biomech.
**1990**, 23, 67–74. [Google Scholar] [CrossRef] - Rotella, N.; Mason, S.; Schaal, S.; Righetti, L. Inertial sensor-based humanoid joint state estimation. In Proceedings of the 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, 16–21 May 2016; pp. 1825–1831. [Google Scholar] [CrossRef]
- Wächter, A.; Biegler, L.T. On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Math. Program.
**2006**, 106, 25–57. [Google Scholar] [CrossRef] - Savitzky, A.; Golay, M. Smoothing and Differentiation of Data by Simplified Least Squares Procedures (ACS Publications). Anal. Chem.
**1964**, 36, 1627–1639. [Google Scholar] [CrossRef] - Featherstone, R. Rigid Body Dynamics Algorithms; Springer US: New York, NY, USA, 2008. [Google Scholar] [CrossRef]
- Latella, C.; Kuppuswamy, N.; Romano, F.; Traversaro, S.; Nori, F. Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing. Sensors
**2016**, 16, 727. [Google Scholar] [CrossRef] - Metta, G.; Fitzpatrick, P.; Natale, L. YARP: Yet Another Robot Platform. Int. J. Adv. Robot. Syst.
**2006**, 3, 8. [Google Scholar] [CrossRef][Green Version] - Nori, F.; Traversaro, S.; Eljaik, J.; Romano, F.; Del Prete, A.; Pucci, D. iCub Whole-Body Control through Force Regulation on Rigid Non-Coplanar Contacts. Front. Robot. AI
**2015**, 2. [Google Scholar] [CrossRef][Green Version]

**Figure 2.**Human model with distributed inertial measurement units. Joint reference frames are shown by using RGB (Red–Green–Blue) convention for x–y–z-axes. (left) Detail of the sensor position on the link.

**Figure 3.**Subject equipped with the Xsens wearable motion tracking system and six-axis force/torque shoes.

**Figure 5.**Classification of feet contacts: (

**left**) single support on the right foot; (

**middle**) double support; and (

**right**) single support on the left foot.

**Figure 6.**Tasks representation from initial time ${t}_{i}$ to final time ${t}_{f}$ (

**left**); and feet contact pattern classification obtained via Algorithm 1 (

**right**).

**Figure 7.**The base linear proper sensor acceleration ${\mathit{\alpha}}_{lin}^{g}$ [$\mathrm{m}/{\mathrm{s}}^{2}$] comparison between measurement (mean and standard deviation, in red) and floating-base MAP estimation (mean, in blue), for tasks T1, T2, T3 and T4.

**Figure 8.**The external force ${\mathit{f}}^{x}$ [N] and moment ${\mathit{m}}^{x}$ [Nm] comparison between measurement (mean and standard deviation, in red) and estimation via floating-base MAP algorithm (mean, in blue) for (

**a**) the left foot and (

**b**) the right foot, respectively, for tasks T1, T2, T3 and T4.

**Figure 9.**Norm of the overall error of the entire set of joint accelerations $\epsilon \left(\ddot{\mathit{s}}\right)$ [$\mathrm{rad}/{\mathrm{s}}^{2}$] between measurement and estimation via floating-base MAP algorithm, for tasks T1, T2, T3 and T4.

**Figure 10.**Floating-base MAP estimation in task T4 of the joint torques $\mathit{\tau}$ [$\mathrm{N}$$\mathrm{m}$] (in blue) for the (

**b**) left leg and (

**a**) right leg, respectively, along with the related joint angles $\mathit{s}$ [deg] (in black). Gait estimations were performed by following the procedure for the feet contact classification in Algorithm 1.

**Figure 11.**Norm of the error of the base proper body linear acceleration $\epsilon \left({\mathit{a}}_{lin}^{g}\right)$ [$\mathrm{m}/{\mathrm{s}}^{2}$] and angular acceleration $\epsilon \left({\mathit{a}}_{ang}^{g}\right)$ [$\mathrm{rad}/{\mathrm{s}}^{2}$] between the estimation with the fixed-base and the floating-base MAP, for tasks T1, T2 and T3. The proper body acceleration for the floating-base estimation is obtained via Equation (13).

**Figure 12.**Norm of the error of the overall set of the overall external forces $\epsilon \left({\mathit{f}}^{x}\right)$ [N] and the moments $\epsilon \left({\mathit{m}}^{x}\right)$ [Nm] between the estimation with the fixed-base and the floating-base MAP, for tasks T1, T2 and T3.

**Figure 13.**Norm of the error of the overall set of joint accelerations $\epsilon \left(\ddot{\mathit{s}}\right)$ [$\mathrm{rad}/{\mathrm{s}}^{2}$] and torques $\epsilon \left(\mathit{\tau}\right)$ [Nm] between the estimation with the fixed-base and the floating-base MAP, for tasks T1, T2 and T3.

Task | Type | Description |
---|---|---|

T1 | Static double support | Neutral pose, standing still |

T2 | Static right single support | Sequence 1: static double support |

Sequence 2: weight balancing on the right foot | ||

T3 | Static left single support | Sequence 1: static double support |

Sequence 2: weight balancing on the left foot | ||

T4 | Static-walking-static | Sequence 1: static double support |

Sequence 2: walking on a treadmill (Figure 4) | ||

Sequence 3: static double support |

**Table 2.**RMSE analysis of the base linear proper sensor acceleration ${\mathit{\alpha}}_{lin}^{g}$ [$\mathrm{m}/{\mathrm{s}}^{2}$], the external force ${\mathit{f}}^{x}$ [N] and moment ${\mathit{m}}^{x}$ [Nm] floating-base algorithm estimations w.r.t. the measurements, for tasks T1, T2, T3 and T4.

Task | Link | ${\mathit{\alpha}}_{\mathit{lin},\mathit{x}}^{\mathit{g}}$ [m/s^{2}] | ${\mathit{\alpha}}_{\mathit{lin},\mathit{y}}^{\mathit{g}}$ [m/s^{2}] | ${\mathit{\alpha}}_{\mathit{lin},\mathit{z}}^{\mathit{g}}$ [m/s^{2}] | ${\mathit{f}}_{\mathit{x}}^{\mathit{x}}$ [N] | ${\mathit{f}}_{\mathit{y}}^{\mathit{x}}$ [N] | ${\mathit{f}}_{\mathit{z}}^{\mathit{x}}$ [N] | ${\mathit{m}}_{\mathit{x}}^{\mathit{x}}$ [Nm] | ${\mathit{m}}_{\mathit{y}}^{\mathit{x}}$ [Nm] | ${\mathit{m}}_{\mathit{z}}^{\mathit{x}}$ [Nm] |
---|---|---|---|---|---|---|---|---|---|---|

Base (Pelvis) | $0.008$ | $0.014$ | $0.002$ | - | - | - | - | - | - | |

T1 | Left foot | - | - | - | $0.050$ | $0.030$ | $2.5\times {10}^{-4}$ | $8.3\times {10}^{-4}$ | $0.002$ | $2.1\times {10}^{-5}$ |

Right foot | - | - | - | $0.031$ | $0.048$ | $0.004$ | 0.0015 | 0.001 | $1.0\times {10}^{-5}$ | |

Base (Pelvis) | $0.003$ | $0.027$ | $0.018$ | - | - | - | - | - | - | |

T2 | Left foot | - | - | - | $0.153$ | $0.071$ | $0.009$ | $0.002$ | $4.7\times {10}^{-4}$ | $1.7\times {10}^{-4}$ |

Right foot | - | - | - | $0.013$ | $0.074$ | $0.005$ | $0.002$ | $4.2\times {10}^{-4}$ | $4.3\times {10}^{-5}$ | |

Base (Pelvis) | $0.012$ | $0.007$ | $0.007$ | - | - | - | - | - | - | |

T3 | Left foot | - | - | - | $0.075$ | $0.019$ | $0.002$ | $6.0\times {10}^{-4}$ | $0.002$ | $5.2\times {10}^{-5}$ |

Right foot | - | - | - | $0.065$ | $0.018$ | $0.003$ | $6.1\times {10}^{-4}$ | $0.002$ | $1.2\times {10}^{-4}$ | |

Base (Pelvis) | $0.011$ | $0.018$ | $0.033$ | - | - | - | - | - | - | |

T4 | Left foot | - | - | - | $0.089$ | $0.056$ | $0.012$ | $0.002$ | $0.003$ | $1.3\times {10}^{-4}$ |

Right foot | - | - | - | $0.084$ | $0.056$ | $0.019$ | $0.002$ | $0.003$ | $9.7\times {10}^{-4}$ |

**Table 3.**Max and min values for RMSE analysis in Table 2, for tasks T1, T2, T3 and T4.

Variables | T1 | T2 | T3 | T4 | ||||
---|---|---|---|---|---|---|---|---|

min | max | min | max | min | max | min | max | |

${\mathit{\alpha}}_{lin,x}^{g}$ | $0.006$ | $0.0086$ | $2.0\times {10}^{-5}$ | $0.0087$ | $0.0029$ | $0.0187$ | $1.6\times {10}^{-7}$ | $0.035$ |

${\mathit{\alpha}}_{lin,y}^{g}$ | $0.012$ | $0.015$ | $0.008$ | $0.053$ | $1.3\times {10}^{-5}$ | $0.0214$ | $1.3\times {10}^{-4}$ | $0.055$ |

${\mathit{\alpha}}_{lin,z}^{g}$ | $1.9\times {10}^{-4}$ | $0.003$ | $0.0062$ | $0.026$ | $0.0015$ | $0.0165$ | $1.3\times {10}^{-5}$ | $0.147$ |

${\mathit{f}}_{LF,x}^{x}$ | $0.045$ | $0.054$ | $0.0016$ | $0.030$ | $0.0071$ | $0.0893$ | $0.0016$ | $0.318$ |

${\mathit{f}}_{LF,y}^{y}$ | $0.026$ | $0.0329$ | $0.0317$ | $0.1258$ | $6.2\times {10}^{-4}$ | $0.042$ | $0.0012$ | $0.144$ |

${\mathit{f}}_{LF,z}^{z}$ | $6.7\times {10}^{-6}$ | $4.8\times {10}^{-4}$ | $2.1\times {10}^{-4}$ | $0.021$ | $5.4\times {10}^{-5}$ | $0.0034$ | $1.0\times {10}^{-5}$ | $0.094$ |

${\mathit{m}}_{LF,x}^{x}$ | $7.3\times {10}^{-4}$ | $9.5\times {10}^{-4}$ | $9.1\times {10}^{-4}$ | $0.004$ | $2.3\times {10}^{-8}$ | $0.0014$ | $1.4\times {10}^{-5}$ | $0.0047$ |

${\mathit{m}}_{LF,x}^{y}$ | $0.0015$ | $0.0013$ | $2.1\times {10}^{-5}$ | $9.4\times {10}^{-4}$ | $3.0\times {10}^{-4}$ | $0.0029$ | $1.6\times {10}^{-5}$ | $0.010$ |

${\mathit{m}}_{LF,x}^{z}$ | $1.7\times {10}^{-5}$ | $2.5\times {10}^{-5}$ | $4.6\times {10}^{-5}$ | $3.0\times {10}^{-4}$ | $3.3\times {10}^{-5}$ | $7.9\times {10}^{-5}$ | $1.5\times {10}^{-7}$ | $7.8\times {10}^{-4}$ |

${\mathit{f}}_{RF,x}^{x}$ | $0.026$ | $0.0362$ | $3.2\times {10}^{-4}$ | $0.038$ | $0.0066$ | $0.077$ | $1.4\times {10}^{-5}$ | $0.296$ |

${\mathit{f}}_{RF,y}^{y}$ | $0.045$ | $0.051$ | $0.033$ | $0.1261$ | $1.5\times {10}^{-4}$ | $0.040$ | $4.3\times {10}^{-6}$ | $0.141$ |

${\mathit{f}}_{RF,z}^{z}$ | $0.0036$ | $0.0045$ | $0.0015$ | $0.014$ | $7.3\times {10}^{-5}$ | $0.0061$ | $5.5\times {10}^{-5}$ | $0.113$ |

${\mathit{m}}_{RF,x}^{x}$ | $0.0014$ | $0.0016$ | $9.8\times {10}^{-4}$ | $0.004$ | $1.5\times {10}^{-5}$ | $0.0014$ | $4.6\times {10}^{-6}$ | $0.005$ |

${\mathit{m}}_{RF,x}^{y}$ | $9.5\times {10}^{-4}$ | $0.0013$ | $2.6\times {10}^{-6}$ | $0.0012$ | $3.3\times {10}^{-4}$ | $0.0029$ | $2.6\times {10}^{-5}$ | $0.0097$ |

${\mathit{m}}_{RF,x}^{z}$ | $6.2\times {10}^{-6}$ | $1.4\times {10}^{-4}$ | $4.0\times {10}^{-6}$ | $6.7\times {10}^{-5}$ | $7.0\times {10}^{-6}$ | $1.7\times {10}^{-4}$ | $5.9\times {10}^{-5}$ | $3.0\times {10}^{-4}$ |

**Table 4.**Mean and standard deviation of the error norm for: $\left(i\right)$ the base proper body linear acceleration related to Figure 11; $\left(ii\right)$ the external wrench related to Figure 12; and $\left(iii\right)$ the joint acceleration and torque related to Figure 13, for tasks T1, T2 and T3.

Error Norm | T1 | T2 | T3 | |
---|---|---|---|---|

$\left|\epsilon \right({\mathit{a}}_{lin}^{g}\left)\right|$ | [$\mathrm{m}/{\mathrm{s}}^{2}$] | $0.008\pm 1.6\times {10}^{-4}$ | $0.0170\pm 0.0176$ | $0.014\pm 0.010$ |

$\left|\epsilon \right({\mathit{a}}_{ang}^{g}\left)\right|$ | [$\mathrm{rad}/{\mathrm{s}}^{2}$] | $0.0357\pm 5.4\times {10}^{-4}$ | $0.024\pm 0.012$ | $0.036\pm 0.005$ |

$\left|\epsilon \right({\mathit{f}}^{x}\left)\right|$ | [$\mathrm{N}$] | $0.026\pm 5.3\times {10}^{-4}$ | $0.033\pm 0.010$ | $0.031\pm 0.006$ |

$\left|\epsilon \right({\mathit{m}}^{x}\left)\right|$ | [$\mathrm{N}\mathrm{m}$] | $9.330{e}^{-4}\pm 1.9\times {10}^{-5}$ | $0.001\pm 3.8\times {10}^{-4}$ | $0.001\pm 3.0\times {10}^{-4}$ |

$\left|\epsilon \right(\ddot{\mathit{s}}\left)\right|$ | [$\mathrm{rad}/{\mathrm{s}}^{2}$] | $0.003\pm 3.7\times {10}^{-5}$ | $0.003\pm 7.4\times {10}^{-4}$ | $0.003\pm 3.0\times {10}^{-4}$ |

$\left|\epsilon \right(\mathit{\tau}\left)\right|$ | [$\mathrm{N}\mathrm{m}$] | $7.199\pm 0.076$ | $3.756\pm 0.845$ | $5.988\pm 0.943$ |

© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Latella, C.; Traversaro, S.; Ferigo, D.; Tirupachuri, Y.; Rapetti, L.; Andrade Chavez, F.J.; Nori, F.; Pucci, D.
Simultaneous Floating-Base Estimation of Human Kinematics and Joint Torques. *Sensors* **2019**, *19*, 2794.
https://doi.org/10.3390/s19122794

**AMA Style**

Latella C, Traversaro S, Ferigo D, Tirupachuri Y, Rapetti L, Andrade Chavez FJ, Nori F, Pucci D.
Simultaneous Floating-Base Estimation of Human Kinematics and Joint Torques. *Sensors*. 2019; 19(12):2794.
https://doi.org/10.3390/s19122794

**Chicago/Turabian Style**

Latella, Claudia, Silvio Traversaro, Diego Ferigo, Yeshasvi Tirupachuri, Lorenzo Rapetti, Francisco Javier Andrade Chavez, Francesco Nori, and Daniele Pucci.
2019. "Simultaneous Floating-Base Estimation of Human Kinematics and Joint Torques" *Sensors* 19, no. 12: 2794.
https://doi.org/10.3390/s19122794