Simultaneous Floating-Base Estimation of Human Kinematics and Joint Torques

Abstract

The paper presents a stochastic methodology for the simultaneous floating-base estimation of the human whole-body kinematics and dynamics (i.e., joint torques, internal and external forces). The paper builds upon our former work where a fixed-base formulation had been developed for the human estimation problem. The presented approach is validated by presenting experimental results of a health subject equipped with a wearable motion tracking system and a pair of shoes sensorized with force/torque sensors while performing different motion tasks, e.g., walking on a treadmill. The results show that joint torque estimates obtained by using floating-base and fixed-base approaches match satisfactorily, thus validating the present approach.

1. Introduction

In physical human–robot interaction (pHRI) domains, a huge variety of applications requires robots to actively collaborate with humans. More and more frequently, robots are required to be endowed with the capability to control physical collaboration through intentional interaction with humans. The simultaneous whole-body estimation of the human kinematics (i.e., motion) and dynamics (i.e., joint torques and internal forces) is a crucial component for modeling, estimating and controlling the interaction. In assistive and rehabilitation scenarios, for instance, the demand for physical robotic assistance to humans is an ever-growing practice and the estimation is a pivotal component for creating technologies capable to help and assist humans. The importance of controlling the pHRI calls for the design of a framework in which the concurrent estimation of the human kinematics and dynamics could be exploited by the robots [1].

In general, several routes for pHRI have been explored over the years. Minimum-jerk-based methods [2,3], imitation learning [4] and retargeting techniques [5,6] are only some of the relevant examples. None of them, however, deal with the simultaneous estimation of the human kinematics and dynamics. Although real-time solutions for whole-body motion tracking are widely marketed (e.g., marker-based motion capture such as Vicon or marker-less systems such as Microsoft Kinect and Xsens wearable suit system), the dynamics estimation is still a challenging problem, especially in those scenarios for which an online estimation is a crucial requirement (e.g., health monitoring or manufacturing ergonomy).

This paper builds upon our former work [7], where a probabilistic algorithm to estimate the human whole-body fixed-base dynamics is described. We propose here a stochastic methodology for the simultaneous floating-base estimation of the human kinematics and dynamics. The estimation is computed by means of a sensor-fusion-based tool able to provide an estimation of the whole-body kinematics and dynamics of the human (torques, internal forces exchanged across joints, and external forces acting on links) by leveraging the reliability of the available measurements. The core of the algorithm has been here adapted to a new version to address the need of the floating-base estimation, in which the position and the velocity of the human base are not assumed to be known a priori. The floating-base formalism is not a novelty in the robotics research and it has been used for multiple topics for years. Inverse dynamics control for humanoids and legged robots [8], modeling and control of humanoids in dynamic environments [9], identification of humanoids inertial parameters [10,11], are only a few examples of applications. In [12,13], the formalism has been even used in the context of human–robot experiments. The human dynamics, however, has been not computed under the floating-base formalism but human motion capture data have been used to generate human-like motions to be retargeted onto floating-base humanoids.

Our experimental validation setup considered a sensorized human subject walking on a treadmill. In general, the gait analysis requires: (i) classifying the human walking state, i.e., the recursive switching pattern from right leg single support → double support → left leg single support; and (ii) defining an algorithm able to detect the pattern classification. In a recent study on the human joint muscular torques estimation during gait [14], two dynamical models have been considered separately for the legs to overcome the problem of the switching contact detection and to avoid the increasing complexity of the control algorithm for pattern classification. In this work, we decided to perform a different choice: we considered the dynamics of the human as a whole (with the pelvis as a floating base) and we developed an algorithm to detect the feet contact via additional sensors readings (force/torque sensors). In general, the algorithm constitutes a powerful and versatile tool to arbitrarily analyze all those tasks for which a switching contact condition is required without changing the base inside the algorithm.

The paper is structured as follows. Section 2 defines the mathematical notation and describes the human kinematics and dynamics modeling. Section 3 describes the steps useful to compute the kinematics and joint torques whole-body estimation for the floating-base formalism. In Section 4, the experimental setup and data analysis are described. Conclusions, limitations and several considerations on future developments are discussed in Section 5.

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])$.
• 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}}{}^{\mathcal{B}}{\mathit{o}}_{\mathcal{L}}$.
• 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}}$, 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

The human is modeled as a rigid multi-body system with $n\in \mathbb{N}$ internal Degrees of Freedom (DoFs). The system is composed of $N_B$ rigid bodies, called links (denoted with $\mathrm{L}$) connected by joints (denoted with $\mathrm{J}$). Links are numbered from 0 to $N_B-1$, being 0 the floating base. Furthermore, $\lambda \left(\mathrm{L}\right)$ and $\mu \left(\mathrm{L}\right)$ represent the parent and child links of $\mathrm{L}$, respectively. The topological order is such that links $\mathrm{L}$ and $\lambda \left(\mathrm{L}\right)$ are coupled by joint $\mathrm{J}$. The joint motion constraint is modeled with the motion freedom subspace $\mathbb{S}\in {\mathbb{R}}^6$. We assume that all the joints have one DoF each, which implies that the joint numbering is from 1 to $n=N_B-1$. No parent joint is assumed for the floating base. See the overall representation in Figure 1.

We also assume that none of the links have a known a priori constant pose w.r.t. $\mathcal{I}$. Thus, we say that the system is floating base. More precisely, the system configuration space is a Lie group $\mathbb{Q}={\mathbb{R}}^3\times SO\left(3\right)\times {\mathbb{R}}^n$ such that $\mathit{q}=({\mathit{q}}_b,\mathit{s})\in \mathbb{Q}$, being ${\mathit{q}}_b=({}^{\mathcal{I}}{\mathit{o}}_{\mathcal{B}},{}^{\mathcal{I}}{\mathit{R}}_{\mathcal{B}})\in {\mathbb{R}}^3\times SO\left(3\right)$ the pose of the base frame $\mathcal{B}$ w.r.t. $\mathcal{I}$ and $\mathit{s}\in {\mathbb{R}}^n$ the joint positions vector capturing the topology of the system. The velocity of the system is represented by $\mathit{\nu }=({}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}},\dot{\mathit{s}})\in {\mathbb{R}}^{6+n}$ being ${}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}=({}^{\mathcal{I}}{\dot{\mathit{o}}}_{\mathcal{B}},{}^{\mathcal{I}}{\mathit{\omega }}_{\mathcal{B}})\in {\mathbb{R}}^6$ the velocity of $\mathcal{B}$ w.r.t. $\mathcal{I}$ (the angular velocity of the base ${}^{\mathcal{I}}{\mathit{\omega }}_{\mathcal{B}}$ is such that ${}^{\mathcal{I}}{\dot{\mathit{R}}}_{\mathcal{B}}=S\left({}^{\mathcal{I}}{\mathit{\omega }}_{\mathcal{B}}\right){}^{\mathcal{I}}{\mathit{R}}_{\mathcal{B}}$). $\dot{\mathit{s}}\in {\mathbb{R}}^n$ is the joint velocities vector. If the system is interacting with the external environment by exchanging $n_c$ wrenches, the dynamics of the floating-base system can be described by adopting the Euler–Poincaré formalism ([15], Ch. 13.5):

$$\begin{array}{c}\hfill \mathit{M}\left(\mathit{q}\right)\dot{\mathbf{\nu }}+\mathit{h}(\mathit{q},\mathbf{\nu })=\mathit{B}\mathbf{\tau }+\sum _{k=1}^{n_c}{\mathit{J}}_{{\mathcal{C}}_k}^{\top }\left(\mathit{q}\right){\mathrm{𝕗}}_k^x,\end{array}$$

(1)

where $\mathit{M}\in {\mathbb{R}}^{\left(n+6\right)\times \left(n+6\right)}$ is the mass matrix, $\mathit{h}\in {\mathbb{R}}^{n+6}$ is a vector accounting for the Coriolis effects and gravity terms, $\mathit{B}:={({\mathbf{0}}_{n\times 6},{\mathbf{1}}_n)}^{\top }$ is a selector matrix, $\mathit{\tau }\in {\mathbb{R}}^n$ represents the joint torques, and ${\mathrm{𝕗}}_k^x=({\mathit{f}}_k^x,{\mathit{m}}_k^x)\in {\mathbb{R}}^6$ is a vector representing the external wrench acting on the system on the link that has the kth contact point, being ${\mathit{f}}_k^x$ and ${\mathit{m}}_k^x$ the external force and moment, respectively. The Jacobian ${\mathrm{J}}_{{\mathcal{C}}_k}\left(\mathit{q}\right)\in {\mathbb{R}}^{6\times (6+n)}$ is an operator that maps the system velocity $\mathit{\nu }$ with the velocity $\mathit{v}$ at the kth contact frame, such that

$$\begin{array}{c}\hfill {{}^{\mathcal{I}}\mathit{v}}_{{\mathcal{C}}_k}={\mathrm{J}}_{{\mathcal{C}}_k}\left(\mathit{q}\right)\mathbf{\nu }=\left[\begin{array}{cc}{\mathbf{J}}_b\left(\mathit{q}\right)& {\mathbf{J}}_s\left(\mathit{q}\right)\end{array}\right]\left[\begin{array}{c}{{}^{\mathcal{I}}\mathit{v}}_{\mathcal{B}}\\ \dot{\mathit{s}}\end{array}\right],\end{array}$$

(2)

with ${\mathbf{J}}_b\left(\mathit{q}\right)\in {\mathbb{R}}^{6\times 6}$ and ${\mathbf{J}}_s\left(\mathit{q}\right)\in {\mathbb{R}}^{6\times n}$ Jacobians being related to the base and joint configuration, respectively.

2.3. Case-Study Human Model

Our case-study human body owns $N_B=67$ links and $n=66$ internal DoFs. The links were modeled with simple geometric shapes (parallelepiped, cylinder, and sphere) whose dimensions were estimated via inertial measurement units (IMUs) readings (i.e, Xsens motion capture system provides the position of several anatomical bony landmarks w.r.t. the origin of each link). The dynamic properties of each link (i.e., inertias and center of mass) were computed via the anthropometric data available in the literature by: (i) exploiting the relation between the total body mass and the mass of each link [16,17]; and (ii) assuming geometric approximations and homogeneous density for the links [18,19].

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

In this section, we describe step-by-step the simultaneous floating-base estimation algorithm for the human whole-body kinematics and dynamics.

3.1. Offline Estimation of Sensor Position

The first objective was to develop a Universal Robot Description Format (URDF) model for the human with properties listed in Section 2.3 (see Figure 2, right). The URDF is a XML-based file format for representing the kinematics and dynamics of multi-body systems. A crucial step for the URDF generation is to identify the position of each sensor w.r.t. the attached link frame (Figure 2, left). Xsens exposes the sensor linear acceleration, the link angular velocity and acceleration, the sensor and link orientation w.r.t. the inertial frame. However, the sensor position is not provided by its framework. A procedure to estimate the sensor position by processing IMUs data was therefore adopted. The procedure is very similar to the one in [20], where it is used for humanoid robots.

More precisely, if $\mathcal{S}$ is the frame associated to the sensor and $\mathcal{L}$ is the frame of the link where the sensor is rigidly attached, then the measurement equation is such that

$$\begin{array}{cc}\hfill {\mathit{a}}_{\mathcal{S}}& ={}^{\mathcal{S}}{\mathit{R}}_{\mathcal{I}}({}^{\mathcal{I}}{\ddot{\mathit{o}}}_{\mathcal{S}}-{}^{\mathcal{I}}\mathit{g})\hfill \\ & ={}^{\mathcal{S}}{\mathit{R}}_{\mathcal{I}}\left[{}^{\mathcal{I}}{\ddot{\mathit{o}}}_{\mathcal{L}}+{}^{\mathcal{I}}{\dot{\mathbf{\omega }}}_{\mathcal{L}}\times {}^{\mathcal{I}}{\mathit{R}}_{\mathcal{L}}{}^{\mathcal{L}}{\mathit{o}}_{\mathcal{S}}+{}^{\mathcal{I}}{\mathbf{\omega }}_{\mathcal{L}}\times ({}^{\mathcal{I}}{\mathbf{\omega }}_{\mathcal{L}}\times {}^{\mathcal{I}}{\mathit{R}}_{\mathcal{L}}{}^{\mathcal{L}}{\mathit{o}}_{\mathcal{S}})-{}^{\mathcal{I}}\mathit{g}\right]\hfill \\ & ={}^{\mathcal{S}}{\mathit{R}}_{\mathcal{I}}\left[{}^{\mathcal{I}}{\ddot{\mathit{o}}}_{\mathcal{L}}+\left(\mathbf{S}\left({}^{\mathcal{I}}{\dot{\mathbf{\omega }}}_{\mathcal{L}}\right)+\mathbf{S}{\left({}^{\mathcal{I}}{\mathbf{\omega }}_{\mathcal{L}}\right)}^2\right){}^{\mathcal{I}}{\mathit{R}}_{\mathcal{L}}{}^{\mathcal{L}}{\mathit{o}}_{\mathcal{S}}-{}^{\mathcal{I}}\mathit{g}\right],\hfill \end{array}$$

(3)

being ${}^{\mathcal{I}}\mathit{g}={[00-9.81]}^{\top }$.

Equation (3) can be rearranged in the following form:

$$\begin{array}{c}\hfill \underset{\mathit{A}}{\underbrace{\left(\mathbf{S}\left({}^{\mathcal{I}}{\dot{\mathbf{\omega }}}_{\mathcal{L}}\right)+\mathbf{S}{\left({}^{\mathcal{I}}{\mathbf{\omega }}_{\mathcal{L}}\right)}^2\right){}^{\mathcal{I}}{\mathit{R}}_{\mathcal{L}}}}{}^{\mathcal{L}}{\mathit{o}}_{\mathcal{S}}=\underset{\mathit{b}}{\underbrace{{}^{\mathcal{I}}{\mathit{R}}_{\mathcal{S}}{\mathit{a}}_{\mathcal{S}}-({}^{\mathcal{I}}{\ddot{\mathit{o}}}_{\mathcal{L}}-{}^{\mathcal{I}}\mathit{g})}}.\end{array}$$

(4)

Given $N_m$ measurements, the position of the sensor w.r.t. the link, i.e., ${}^{\mathcal{L}}{\mathit{o}}_{\mathcal{S}}$, is the solution of the following optimization problem:

$$\mathcal{L}{\mathit{o}}_{\mathcal{S}}^{*}=arg\underset{\mathcal{L}{\mathit{o}}_{\mathcal{S}}}{min}{|\overline{\mathit{A}}\mathcal{L}{\mathit{o}}_{\mathcal{S}}-\overline{\mathit{b}}|}^2$$

(5)

being $\overline{\mathit{A}}={[{\mathit{A}}_1\hspace{1em}{\mathit{A}}_2\dots {\mathit{A}}_{N_m}]}^{\top }$, $\overline{\mathit{b}}={[{\mathit{b}}_1\hspace{1em}{\mathit{b}}_2\dots {\mathit{b}}_{N_m}]}^{\top }$.

3.2. Estimation of Human Kinematics

The objective of this section is to derive algorithms for estimating the human kinematic configuration $\mathit{q}=({\mathit{q}}_b,\mathit{s})$ and its derivatives.

Per each link pair $\left[\lambda \right(\mathrm{L}),\mathrm{L}]$ coupled by the joint J, $\mathit{s}\in {\mathbb{R}}^n$ was computed by solving an optimization problem using Ipopt [21]. The problem is formulated to minimize the distance between the measured, i.e., ${}^{\lambda \left(\mathrm{L}\right)}{\mathit{R}}_{\mathrm{L}}^{meas}$, and the computed, i.e., ${}^{\lambda \left(\mathrm{L}\right)}{\mathit{R}}_{\mathrm{L}}$, relative rotations between the frames attached to the link pair, such that

$$s_{\mathrm{J}}=arg\underset{s}{min}distance\left({}^{\lambda \left(\mathrm{L}\right)}{\mathit{R}}_{\mathrm{L}}^{meas},{}^{\lambda \left(\mathrm{L}\right)}{\mathit{R}}_{\mathrm{L}}\right),s.t.s_{\mathrm{J},min}<s_{\mathrm{J}}<s_{\mathrm{J},max}$$

(6)

being the distance defined in terms of the rotation error parameterized in Euler angles, and $(s_{\mathrm{J},min},s_{\mathrm{J},max})$ joint limits. We refer to Equation (6) as a link-pairwise inverse kinematics (IK) problem. Joint velocities and accelerations $\dot{\mathit{s}},\ddot{\mathit{s}}\in {\mathbb{R}}^n$ were computed by using a weighted sum of moving windows of elements with a third-order polynomial Savitzky–Golay filtering [22].

The base pose ${\mathit{q}}_b$ was obtained via IMUs readings. The pivotal modification for the floating-base formalism deals with the computation of the velocity ${}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}$ of the floating base. It is assumed that holonomic constraints in the form of $c\left(\mathit{q}\right)=0$ act on the system in Equation (1). In the human experimental framework, constraints occur when the system is in contact with the ground such that the feet can be considered as end-effectors with zero velocity (i.e., ${{}^{\mathcal{I}}\mathit{v}}_{{\mathcal{C}}_k}=\mathbf{0}$). This yields to

$$\begin{array}{c}\hfill \mathbf{0}={\mathrm{J}}_{{\mathcal{C}}_k}\left(\mathit{q}\right)\mathbf{\nu }.\end{array}$$

(7)

If $\mathcal{RF}$ and $\mathcal{LF}$ are the contact frames associated to the right and left foot, respectively, for Equation (2), we can write Equation (7) such that

$$\begin{array}{ccc}\hfill \mathbf{0}& =& {\mathbf{J}}_{b,\mathcal{RF}}\left(\mathit{q}\right){{}^{\mathcal{I}}\mathit{v}}_{\mathcal{B}}+{\mathbf{J}}_{s,\mathcal{RF}}\left(\mathit{q}\right)\dot{\mathit{s}},\hfill \end{array}$$

(8a)

$$\begin{array}{ccc}\hfill \iff {}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}^{*}& =& arg\underset{{}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}}{min}|{\mathbf{J}}_{b,\mathcal{RF}}\left(\mathit{q}\right){}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}+{\mathbf{J}}_{s,\mathcal{RF}}\left(\mathit{q}\right)\dot{\mathit{s}}{|}^2,\hfill \end{array}$$

(8b)

if only the contact in $\mathcal{RF}$ occurs, and

$$\begin{array}{ccc}\hfill \mathbf{0}& =& {\mathbf{J}}_{b,\mathcal{LF}}\left(\mathit{q}\right){{}^{\mathcal{I}}\mathit{v}}_{\mathcal{B}}+{\mathbf{J}}_{s,\mathcal{LF}}\left(\mathit{q}\right)\dot{\mathit{s}},\hfill \end{array}$$

(9a)

$$\begin{array}{ccc}\hfill \iff {}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}^{*}& =& arg\underset{{}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}}{min}|{\mathbf{J}}_{b,\mathcal{LF}}\left(\mathit{q}\right){}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}+{\mathbf{J}}_{s,\mathcal{LF}}\left(\mathit{q}\right)\dot{\mathit{s}}{|}^2,\hfill \end{array}$$

(9b)

if the contact occurs in $\mathcal{LF}$.

Nevertheless, if the system is simultaneously constrained by both feet, we need to consider the overall effect on the system,

$$\begin{array}{ccc}\hfill \mathbf{0}& =& \underset{{\overline{\mathbf{J}}}_{b\left(\mathit{q}\right)}}{\underbrace{\left[\begin{array}{c}{\mathrm{J}}_{b,\mathcal{RF}}\left(\mathit{q}\right)\\ {\mathrm{J}}_{b,\mathcal{LF}}\left(\mathit{q}\right)\end{array}\right]}}{}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}\underset{{\overline{\mathbf{J}}}_{b\left(\mathit{q}\right)}}{\underbrace{\left[\begin{array}{c}{\mathrm{J}}_{s,\mathcal{RF}}\left(\mathit{q}\right)\\ {\mathrm{J}}_{s,\mathcal{LF}}\left(\mathit{q}\right)\end{array}\right]}}\dot{\mathit{s}}\hfill \end{array}$$

(10a)

$$\iff {}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}^{*}=arg\underset{{}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}}{min}{|{\overline{\mathbf{J}}}_b\left(\mathit{q}\right){}^{\mathcal{I}}{\mathit{v}}_{\mathcal{B}}+{\overline{\mathbf{J}}}_s\left(\mathit{q}\right)\stackrel{·}{s}|}^2$$

(10b)

3.3. Offline Contact Classification

We implemented an offline algorithm to detect which foot is in contact with the ground, i.e., double support state, left single support state, or right single support state (see Algorithm 1). The contact classification is determined via force/torque (FT) sensors readings and depends on a self-tuned threshold force value $T_{f_z}$. The threshold defines how big the area of the double support has to be considered. When a single support occurs, the algorithm is able to classify which foot is in contact with the ground by reading and comparing the FT sensors values.

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 fz = mean(${}^{\mathcal{R}F}{\mathit{f}}_z+{}^{\mathcal{L}F}{\mathit{f}}_z$)
4:	main loop:
5:	for $j=1\to N$ do
6:	if $abs\left({}^{\mathcal{R}F}{\mathit{f}}_z-{}^{\mathcal{L}F}{\mathit{f}}_z\right)\le T_{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

The simultaneous estimation of the human kinematics and dynamics is performed by means of a Maximum-A-Posteriori (MAP) algorithm. The advantages of this algorithm are discussed in [7]. Here, the objective is to describe how the core of the algorithm was modified to fit the floating-base formalism. The main difference lies in a new representation for the acceleration. Instead of using the proper body acceleration $\in {\mathbb{R}}^6$ (e.g., as in [23]), i.e.,

$$\begin{array}{c}\hfill {\mathit{a}}_{\mathcal{L}}^g={\dot{\mathit{v}}}_{\mathcal{L}}-\left[\begin{array}{c}{}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}{}^{\mathcal{I}}\mathit{g}\\ {\mathbf{0}}_{3\times 1}\end{array}\right]=\left[\begin{array}{c}{}^{\mathcal{I}}{\ddot{\mathit{o}}}_{\mathcal{L}}\\ {}^{\mathcal{I}}{\dot{\mathbf{\omega }}}_{\mathcal{L}}\end{array}\right]-\left[\begin{array}{c}{}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}{}^{\mathcal{I}}\mathit{g}\\ {\mathbf{0}}_{3\times 1}\end{array}\right],\end{array}$$

(11)

we decided to adopt the proper sensor acceleration $\in {\mathbb{R}}^6$, i.e.,

$$\begin{array}{ccc}\hfill {\mathbf{\alpha }}_{\mathcal{L}}^g& =\hfill & {\mathbf{\alpha }}_{\mathcal{L}}-\left[\begin{array}{c}{}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}{}^{\mathcal{I}}\mathit{g}\\ {\mathbf{0}}_{3\times 1}\end{array}\right]={}^{\mathcal{L}}{\mathit{X}}_{\mathcal{L}\left[\mathcal{I}\right]}{}^{\mathcal{L}\left[\mathcal{I}\right]}{\dot{\mathit{v}}}_{\mathcal{L}}-\left[\begin{array}{c}{}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}{}^{\mathcal{I}}\mathit{g}\\ {\mathbf{0}}_{3\times 1}\end{array}\right]\hfill \\ & =\hfill & \left[\begin{array}{cc}{}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}\end{array}\right]\left[\begin{array}{c}{}^{\mathcal{I}}{\ddot{\mathit{o}}}_{\mathcal{L}}\\ {}^{\mathcal{I}}{\dot{\mathbf{\omega }}}_{\mathcal{L}}\end{array}\right]-\left[\begin{array}{c}{}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}{}^{\mathcal{I}}\mathit{g}\\ {\mathbf{0}}_{3\times 1}\end{array}\right]=\left[\begin{array}{c}{}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}{}^{\mathcal{I}}{\ddot{\mathit{o}}}_{\mathcal{L}}\\ {\dot{\mathbf{\omega }}}_{\mathcal{L}}\end{array}\right]-\left[\begin{array}{c}{}^{\mathcal{L}}{\mathit{R}}_{\mathcal{I}}{}^{\mathcal{I}}\mathit{g}\\ {\mathbf{0}}_{3\times 1}\end{array}\right].\hfill \end{array}$$

(12)

being $\mathit{X}\in {\mathbb{R}}^{6\times 6}$ the adjoint transformation matrix for motion vectors. The main advantage is that the linear part of Equation (12) corresponds to Equation (3) where the frame of the accelerometer coincides with the frame $\mathcal{L}$ of the link (same origin and orientation). In general, several modifications were required for the floating-base formalism as follows.

• 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\mathrm{being}{\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]$$

  (13)
• 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}}^{12N_B+7n},\hfill \end{array}$$

  (14)

  being

  $$\begin{array}{ccc}\hfill {\mathit{d}}_{link}& =& \left[\begin{array}{ccccccc}{\mathbf{\alpha }}_0^g& {\mathrm{𝕗}}_0^x& {\mathbf{\alpha }}_1^g& {\mathrm{𝕗}}_1^x& \cdots & {\mathbf{\alpha }}_{N_B-1}^g& {\mathrm{𝕗}}_{N_B-1}^x\end{array}\right]\in {\mathbb{R}}^{12N_B},\hfill \end{array}$$

  (15a)

  $$\begin{array}{ccc}\hfill {\mathit{d}}_{joint}& =& \left[\begin{array}{cccccccc}{\mathrm{𝕗}}_1& {\mathrm{𝕗}}_2& \cdots & {\mathrm{𝕗}}_n& {\ddot{s}}_1& {\ddot{s}}_2& \cdots & {\ddot{s}}_n\end{array}\right]\in {\mathbb{R}}^{7n}.\hfill \end{array}$$

  (15b)

  In the new serialization, ${\mathit{\alpha }}^g\in {\mathbb{R}}^6$ is the proper sensor acceleration of Equation (12) and ${\mathrm{𝕗}}^x\in {\mathbb{R}}^6$ is the external wrench acting on each link. Similarly for the joint quantities, $\mathrm{𝕗}\in {\mathbb{R}}^6$ is the internal wrench (or joint wrench) exchanged from $\lambda \left(\mathrm{L}\right)$ to $\mathrm{L}$ through the joint $\mathrm{J}$, while $\ddot{s}$ is the joint acceleration.
• 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{𝕗}}_{\mathrm{J}}$, for each joint $\mathrm{J}$ of the model.

Within this new formalism, we rewrite the equation of the acceleration propagation and the recursive Newton–Euler equations. The Newton–Euler formalism is an equivalent representation of Equation (1) (more details about this choice can be found in Section 3.3 of [24]). For the sake of simplicity, here following, we refer to $\mathrm{L}$ and $\mathrm{J}$ as compact forms for ${\mathcal{L}}_{\mathrm{L}}$ and ${\mathcal{J}}_{\mathrm{J}}$, respectively, thus:

$$\begin{array}{c}\hfill {\mathbf{\alpha }}_{\mathrm{L}}^g={}^{\mathrm{L}}{\mathit{X}}_{\lambda \left(\mathrm{L}\right)}{}^{\lambda \left(\mathrm{L}\right)}{\mathbf{\alpha }}_{\lambda \left(\mathrm{L}\right)}^g+{\mathbb{S}}_{\mathrm{J}}{\ddot{\mathit{s}}}_{\mathrm{J}}+\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}}+({\overline{\mathbf{\alpha }}}_{\mathrm{L}}^g-{}^{\mathrm{L}}{\mathit{X}}_{\lambda \left(\mathrm{L}\right)}{}^{\lambda \left(\mathrm{L}\right)}{\overline{\mathbf{\alpha }}}_{\lambda \left(\mathrm{L}\right)}^g)\end{array}$$

(16)

$${\mathit{M}}_{\mathrm{L}}{\mathbf{\alpha }}_{\mathrm{L}}^g+\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]={\mathrm{𝕗}}_{\mathrm{L}}^x+{\mathrm{𝕗}}_{\mathrm{J}}-{\sum }^{\mathrm{J}}{X}_{\mathrm{L}\mu }^{*}{\mathrm{𝕗}}_{\mathrm{L}\mu },$$

(17)

where ${\mathit{X}}^{*}\in {\mathbb{R}}^{6\times 6}$ and ${\times }^{*}$ are the adjoint transformation matrix and the dual cross product operator $\in {\mathbb{R}}^3$ for force vectors, respectively. In general, these equations seem to be much more complex than the ones obtained by using the proper body acceleration in [7]. They have, however, the convenient property to be agnostic to the linear velocity of each link. This property drastically simplifies the generalization to the floating-base case, in which the linear velocity of the floating base is, in general, not available.

As already described in [24], the estimation problem can be compactly arranged in a matrix form, as follows:

$$\begin{array}{c}\hfill \left[\begin{array}{c}\mathit{Y}\left(\mathit{s}\right)\\ \mathit{D}\left(\mathit{s}\right)\end{array}\right]\mathit{d}+\left[\begin{array}{c}{\mathit{b}}_Y(\mathit{s},\dot{\mathit{s}})\\ {\mathit{b}}_D(\mathit{s},\dot{\mathit{s}})\end{array}\right]=\left[\begin{array}{c}\mathit{y}\\ {\mathit{0}}_{12N_B\times 1}\end{array}\right].\end{array}$$

(18)

More in detail:

• The first set of equations $\mathit{Y}\left(\mathit{s}\right)\mathit{d}+{\mathit{b}}_Y(\mathit{s},\dot{\mathit{s}})=\mathit{y}$ 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[{\mathbf{\omega }}_{\mathcal{B}}\times \left({\mathbf{\omega }}_{\mathcal{B}}\times {}^{\mathcal{B}}{\mathit{o}}_{IMU}\right)\right]\\ {\mathbf{0}}_{3\times 1}\end{array}\right].\end{array}$$

  (19)

  Similarly, for the FT sensor frames rigidly connected to the feet frames, the measurement equation is

  $$\begin{array}{c}\hfill {\mathit{y}}_{\mathcal{L},FT}={}^{\mathcal{L}}{\mathit{X}}_{FT}^{*}{\mathrm{𝕗}}_{FT}.\end{array}$$

  (20)
• The second set of equations $\mathit{D}\left(\mathit{s}\right)\mathit{d}+{\mathit{b}}_D(\mathit{s},\dot{\mathit{s}})=\mathbf{0}$ 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}}^{12N_B\times d}$ is a matrix with $12N_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,\hfill \end{array}$$

  (21a)

  $$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathbf{\alpha }}_{\lambda \left(\mathrm{L}\right)}}& =& \left\{\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}\right.\hfill \end{array}$$

  (21b)

  $$\begin{array}{ccc}\hfill {\mathit{D}}_{{\ddot{\mathit{s}}}_{\mathrm{J}}}& =& \left\{\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}\right.\hfill \end{array}$$

  (21c)

  The blocks in $\mathit{D}$ for Newton–Euler equations related to Equation (17) are instead:

  $$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathbf{\alpha }}_{\mathrm{L}}}& =& {\mathit{M}}_{\mathrm{L}}\in {\mathbb{R}}^{6\times 6},\hfill \end{array}$$

  (22a)

  $$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathit{f}}_{\mathrm{L}}^x}& =& -{\mathbf{1}}_6,\hfill \end{array}$$

  (22b)

  $$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathit{f}}_{\mathrm{J}}}& =& \left\{\begin{array}{cc}-{\mathbf{1}}_6\hfill & \forall \mathrm{L}\ne \mathcal{B}\hfill \\ {\mathbf{0}}_6\hfill & otherwise\hfill \end{array}\right.\hfill \end{array}$$

  (22c)

  $$\begin{array}{ccc}\hfill {\mathit{D}}_{{\mathit{f}}_{{\mathrm{J}}_{\mu }}}& =& \left\{\begin{array}{cc}{}^{\mathrm{J}}{\mathit{X}}_{{\mathrm{J}}_{\mu }}^{*}\in {\mathbb{R}}^{6\times 6}\hfill & if\exists \mu \left(\mathrm{L}\right)\hfill \\ {\mathbf{0}}_6\hfill & otherwise\hfill \end{array}\right.\hfill \end{array}$$

  (22d)

  All the other blocks in $\mathit{D}$ are equal to 0. Unlike $\mathit{D}$, the term ${\mathit{b}}_D\in {\mathbb{R}}^{12N_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}{cc}\left[\begin{array}{c}{\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}\right]& if\hspace{1em}\mathrm{L}=\mathcal{B}\\ \left[\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.{\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]{\overline{\lambda }}_{\left(\mathrm{L}\right)}^g\\ \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]\end{array}\right]& otherwise\end{array}$$

  (23)

The solution of the system in Equation (18) is computed in a Gaussian domain via MAP estimator. Within this framework, 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.,

$${\mathit{d}}_{MAP}=arg\underset{\mathit{d}}{max}p\left(\mathit{d}\right|\mathit{y}).$$

(24)

Equation (24) corresponds to the mean of the conditional probability ${\mathit{\mu }}_{d|y}$, such that

$$\begin{array}{ccc}\hfill {\mathbf{\Sigma }}_{d|y}& =& {\overline{\mathbf{\Sigma }}}_D^{-1}+{\mathit{Y}}^{\top }{\mathbf{\Sigma }}_y^{-1}{\mathit{Y}}^{-1},\hfill \end{array}$$

(25a)

$${\mathbf{\mu }}_{d|y}={\mathbf{\Sigma }}_{d|y}\left[{\mathit{Y}}^{\top }{\mathbf{\Sigma }}_y^{-1}(y-{\mathit{b}}_Y)+{\overline{\mathbf{\Sigma }}}_D^{-1}{\overline{\mu }}_D\right].$$

(25b)

More details on the MAP solution are provided in Appendix A.

4. Experiments and Analysis

4.1. Experimental Setup

The objective of the experiment was to test the goodness of the estimation algorithm. An experimental session was carried out at Istituto Italiano di Tecnologia (IIT), Genoa, Italy, with a healthy male subject. The participant was equipped with an Xsens wearable motion tracking system with 17 IMUs to capture the whole-body kinematics. A pair of sensorized shoes developed at IIT was used to detect the ground reaction forces. Each shoe was equipped with two six-axis FT sensors able to measure 6D wrenches (3 forces and 3 moments), as shown in Figure 3. The subject was asked to perform a set of different tasks, as listed in

Table 1. Tasks performed for the estimation analysis.

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

.

Data were recorded at 50 Hz via a YARP-based [25] framework for wearable sensors that allows synchronously collecting data coming from multiple sources (see open-source code available on Github repository https://github.com/robotology-playground/wearables). Data processing was analyzed on MathWorks MATLAB^®. 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).

Data coming from the shoes FT sensors were analyzed to detect the feet contact, as described in Algorithm 1 in Section 3.3. The overall estimation considered thus Equation (10b) for T1, Equation (8b) for Sequence 2 of T2, and Equation (9b) for Sequence 2 of T3. In Sequence T4 on Figure 4, for instance, the algorithm detected the switching contact condition of the feet in Figure 5 and applied the proper base velocity computation. Figure 6 shows the feet contact detection for the tasks in Table 1.

4.2. Comparison between Measurement and Estimation

The primary objective of the floating-base MAP algorithm is to estimate simultaneously kinematic and dynamic quantities related to the links and the joints of the human model. The vector 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{𝕗}$ 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. 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/s2]	${\mathit{\alpha }}_{\mathit{lin},\mathit{y}}^{\mathit{g}}$ [m/s2]	${\mathit{\alpha }}_{\mathit{lin},\mathit{z}}^{\mathit{g}}$ [m/s2]	${\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}$

). Error range values are shown in

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}$

.

It is worth remarking on the importance of the choice of the covariance matrix associated to the sensors. It can be manually tuned as a parameter of the measurement trust. In this experimental analysis, covariances were chosen in a range from ${10}^{-6}$ to ${10}^{-4}$: the higher the level of sensor trust (i.e., low covariance), the lower the RMSE associated to the sensor variable.

Figure 9 represents the norm of the overall error of the joint acceleration $\ddot{\mathit{s}}$ [$\mathrm{rad}/{\mathrm{s}}^2$], for tasks T1, T2, T3 and T4. The error norm $\left|\epsilon \left(\ddot{\mathit{s}}\right)\right|=\left|\left({\ddot{\mathit{s}}}_{measured}-{\ddot{\mathit{s}}}_{estimated}\right)\right|$ was computed by considering the entire set of joints of the model, such that $\left|{\epsilon }_{T1}\left(\ddot{\mathit{s}}\right)\right|=0.007\pm 4.9\times {10}^{-5}$ [$\mathrm{rad}/{\mathrm{s}}^2$], $\left|{\epsilon }_{T2}\left(\ddot{\mathit{s}}\right)\right|=0.008\pm 0.001$ [$\mathrm{rad}/{\mathrm{s}}^2$], $\left|{\epsilon }_{T3}\left(\ddot{\mathit{s}}\right)\right|=0.007\pm 3.5\times {10}^{-4}$ [$\mathrm{rad}/{\mathrm{s}}^2$], $\left|{\epsilon }_{T4}\left(\ddot{\mathit{s}}\right)\right|=0.009\pm 0.002$ [$\mathrm{rad}/{\mathrm{s}}^2$].

4.3. Human Joint Torques Estimation during Gait

The floating-base MAP algorithm provides the whole-body joint torque estimation. The estimated torque does not have a measured quantity to be compared. We can trust only the estimation as a consequence of the validation analysis in Section 4.2. The algorithm becomes particularly useful when dealing with the human gait analysis. Figure 10 shows the joint torque estimations along with the joint angles for the ( Figure 10a) left leg and ( Figure 10b) right leg, respectively. We decided, here, to show only the most representative results for the walking task.

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

We performed a comparison between the floating-base estimation w.r.t. the fixed-base estimation done in [24] by computing the norm of the error between the two formalisms, for tasks T1, T2 and T3. In general, the error norm was computed as the norm of the difference between each fixed-base and the floating-base estimated variable, i.e, $\left|\epsilon \left(variable\right)\right|=\left|(variabl{e}_{estimatedFixed}-variabl{e}_{estimatedFloating})\right|$. Figure 11 shows the norm of the error for 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$] of the base (i.e, the Pelvis) between the estimations with the formalisms. The proper body acceleration for the floating-base MAP estimation was obtained via Equation (13). The same comparison was performed for the overall set of external force $\epsilon \left({\mathit{f}}^x\right)$ [N] and moment $\epsilon \left({\mathit{m}}^x\right)$ [Nm] errors (Figure 12), and for the entire set of joint acceleration $\epsilon \left(\ddot{\mathit{s}}\right)$ [$\mathrm{rad}/{\mathrm{s}}^2$] and torque $\epsilon \left(\mathit{\tau }\right)$ [Nm] (Figure 13) errors.

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.330e^{-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$

shows the mean and standard deviation of the error norms.

In addition to the analytical modifications for the floating-base formalism described in Section 3.4, there is another important advantage in using the floating-base MAP. Unlike the fixed-base estimation where it is fundamental to change the base among the tasks (left foot for tasks T1 and T3, and right foot for T2), this limitation does not exist for the floating-base algorithm, e.g., the pelvis remains the base for all the tasks. Furthermore, the floating-base formalism allows us to make up for the lack of the external force estimation that exists for the fixed-base algorithm on the link appointed as the model base.

4.5. A Word of Caution on the Covariances Choice

The MAP algorithm estimation depends on the covariance values chosen by the end-user. In Equations (25a) and (25b), it is visible the role of: (i) the measurements covariance ${\mathbf{\Sigma }}_y$; and $\left(ii\right)$ a covariance ${\overline{\mathbf{\Sigma }}}_D$ that, in turn, takes into account the model reliability (via covariance ${\mathbf{\Sigma }}_D$) and the prior on the estimation (via covariance ${\mathbf{\Sigma }}_d$) (see details in Appendix A, Equations (A5a) and (A5b)). In general, the procedural approach consists in assigning

• 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.

The combined contribution of this set of covariances affects the final estimation. The estimation vector $\mathit{d}$ contains variables that are also measured (e.g., linear acceleration, external wrench, and joint acceleration) for which the covariance of the measurement ${\mathbf{\Sigma }}_y$ plays a predominant role (a minor role is due to the covariance ${\mathbf{\Sigma }}_d$ of the prior). A problem arises, however, when considering the angular acceleration ${\mathit{\alpha }}_{ang}^g$. This variable is part of the $\mathit{d}$ vector as estimation, but it is not measured. At the current stage, a way to play with the angular acceleration trust is, therefore, to tune ${\mathbf{\Sigma }}_d$. A forthcoming investigation will deal with the possibility of integrating the angular acceleration as part in the vector $\mathit{y}$ of the measurements.

5. Conclusions

In this paper, we present a stochastic methodology for the simultaneous floating-base estimation of the whole-body human kinematics and dynamics (joint torques, internal forces and external forces). The novelty consists in the possibility to perform the estimation in a floating-base framework. The floating base can be arbitrarily chosen among the model links and the algorithm requires to estimate the pose and 6D velocity of the base w.r.t. the inertial frame.

The algorithm was validated by carrying out a four-task experimental session with a healthy subject equipped with a wearable motion tracking system—to capture the whole-body kinematics—and a pair of force/torque sensorized shoes. We performed the tasks in Table 1 by considering the human pelvis as the system (floating) base. This choice came particularly in handy with the walking task on the treadmill. In general, the algorithm allows analyzing all those tasks for which a switching contact condition is implicitly required (e.g., the human gait) without manually tuning the model base into the algorithm.

Current limitations of the methodology concern: (i) the human URDF generation and the estimation of sensors position w.r.t. the attached link (see Section 3.1); and (ii) the feet contact classification (see Section 3.3), as they are carried out in an offline post-processing step. The impeding objective is to develop a new online procedure to automatize the human model generation from real-time acquisitions together with a real-time algorithm to classify the feet contact. These two features will strongly improve the already existing tool for the online estimation of the human joint torques (open-source code available on Github repository https://github.com/robotology/human-dynamics-estimation).