Implement human joint velocities for paired-wise and whole-body ipopt based IK

[yeshasvitirupachuri]

Currently, we are setting the joint velocities to zero manually. We need to implement correct joint velocities computation based on the information from the xsens suit.

• [x] First we need to understand how it was implemented in HDEv1 (@claudia-lat mentioned that @lucaTagliapietra implemented a state space filter )
• [x] See what information is needed from the xsens suit through wearables here
• [x] Implement the part of joint velocities computation

@lrapetti @kouroshD @claudia-lat @diegoferigo

[lrapetti]

• [x] First we need to understand how it was implemented in HDEv1 (@claudia-lat mentioned that @lucaTagliapietra implemented a state space filter )

The approach used in the past for computing joint velocities and joint accelerations are explained below.

joint velocities ($\dot{q}$)

(ref https://github.com/robotology/human-dynamics-estimation/blob/master/human-state-provider/src/HumanIKWorkerPool.cpp#L95) The joint velocities are computed in the human-state-provider pairwised (in each thread we compute the joint acceleration for the i-th multi-dimensional joint $\dot{q}_i$ such as for joint angles). So for each couple of links we have:

• joint state for i-th multi-dimensional joint ($q_i$) ($q_i \in \mathbb{R}^3$ for spherical joints)
• parent/child link velocity ($\mathcal{v}_p$,$\mathcal{v}_c$)
• human model

The velocity is then computed with the following step:

• get the relative Jacobian for the two links given the model and the current joint configuration ($J_i(q_i)$).
• compute the relative velocity between parent and child link as: $$ \mathcal{v}_i = \mathcal{v}_c - \mathcal{v}_p $$
• from the definition of the jacobian, we can pseudo-invert it in order to compute the joints velocities for the i-th angle. $$ \mathcal{v}_i = J_i \dot{q}_i \\ \to \dot{q}_i=J_i^{\dagger} \mathcal{v}_i $$

Joint accelerations ($\ddot{q}$)

(ref https://github.com/robotology/human-dynamics-estimation/commit/789f3cdb5bf797d5e080baf99b25b5f1b00552db) The joint acceleration was estimated directly in the human-dynamics-estimator by filtering the joints velocity data ($\dot{q}$). The filter used is the one in https://github.com/RealTimeBiomechanics/Filter, and a reference paper on that is https://www.researchgate.net/publication/238835613_Accurate_derivative_estimation_from_noisy_data_a_state-space_approach.

[lrapetti]

• [x] See what information is needed from the xsens suit through wearables here

All the information used in the previous version is still available. We can see that from the xsens suit we are still getting the (virtual) links velocities (https://github.com/Yeshasvitvs/wearables/blob/master/devices/XsensSuit/src/XsensSuit.cpp#L479).

[lrapetti]

I think as a first instance we can implement joint velocities as in the previous implementation. As for the joint acceleration, currently, we are not using them as an input of the Berdy algorithm so there is no need to compute them (but it may be useful in the future).

[lrapetti]

I did a preliminary implementation in https://github.com/Yeshasvitvs/human-dynamics-estimation/commit/7e2978b71ea9e5b44173abf8cf0113e439cdf34c. I see the joint velocities are streamed, we should check if the values obtained are acceptable (we could compare it with the offline estimation).

computeJointVelocities() was actually already implemented when porting the WarkerPool from the old implementation, but the link velocities were all set to zero and that's why we were computing zero joint velocities. In my commit, I have implemented the reading of link velocities data (getLinkVelocityFromInputData()) and I am passing the computed joint velocities from the WalkerPool to the output.

Concerning the velocity of the floating base link, I am directly using the velocity given by the Xsens (see solution.baseVelocity).

[DanielePucci]

Nice @lrapetti. Let's talk F2F about this. Another possibility may be that using all the angular velocity measurements stuck together and do a whole body least square. Probably, this could also lead to another way to get human joint angles through a sort of integration based IK plus proper initialisation

[traversaro]

I guess this is partially related to @nunoguedelha's work.

[nunoguedelha]

Nice @lrapetti. Let's talk F2F about this. Another possibility may be that using all the angular velocity measurements stuck together and do a whole body least square. Probably, this could also lead to another way to get human joint angles through a sort of integration based IK plus proper initialisation

Probably that's what you meant, but it can still be a linear least-squares, i.e a pseudo-inverse for the whole body (just the joints, so excluding the floating base). I mean, we can formulate the problem having a vector $v$ of all the joint relative velocities $v_i$, a vector $\dot{q}$ of all the generalized velocities $\dot{q}_i$, and a block diagonal matrix $J$ with all the relative jacobians $J_i$, then pose: $$ \dot{q} = J^{\dagger} v $$

[nunoguedelha]

Joint accelerations ($\ddot{q}$)

... The filter used is the one in https://github.com/RealTimeBiomechanics/Filter, and a reference paper on that is https://www.researchgate.net/publication/238835613_Accurate_derivative_estimation_from_noisy_data_a_state-space_approach.

I would be glad, when the time comes, to try integrating in that filter the model-based direct link acceleration computation (without numerical derivation) I've been working on. We could get the joint accelerations with a small change in that computation, or just using the same approach as for the velocities: $$ \begin{align} a_i = a_c - a_p \newline a_i - \dot{J}_i \dot{q}_i = J_i \ddot{q}_i \newline \ddot{q}_i = J_i^{\dagger} \left( a_i - \dot{J}_i \dot{q}_i \right) \newline \end{align} $$ where the relative jacobian with respect to joint $i$ is actually the joint space vector $J_i = s_i$.

Btw, @lrapetti do you have a copy of that paper? I've requested the full-text but it could be long to get an answer..

[lrapetti]

or just using the same approach as for the velocities: $$ \begin{align} a_i = a_c - a_p \newline a_i - \dot{J}_i \dot{q}_i = J_i \ddot{q}_i \newline \ddot{q}_i = J_i^{\dagger} \left( a_i - \dot{J}_i \dot{q}_i \right) \newline \end{align} $$

I was thinking the same. It may be worth to try to use the same approach for the aceleration (it should come out quite easy to be tested).

[nunoguedelha]

I just forgot to mention that in (10) (11) and (12) the $a_i$, $a_c$ and $a_p$ are spatial accelerations that have to be computed, if not already done in your framework.

[DanielePucci]

@nunoguedelha Yes, what I meant is given basically what you wrote above. More precisely, what I was suggesting for human joint velocity and position estimation is: 1) Perform nonlinear optimisation to get $q_0$ 2) Obtain human velocities from least squares, i.e. $\nu = J^{\dagger}\omega$, where $\omega$ is a vector stacking all angular velocities measured from the sensors 3) Smart integrate $\nu$ using $q_0$ obtained in (1 as initialisation to get $q(t)$. Here, correction terms that depend on obtained (from integration) and measured rotation matrixes are fundamental to be added. So, the integration will integrate $\nu = J^{\dagger}(\omega + \text{correctionTerms})$

This is something that it is basically already in place for the walking controllers (integration based IK) and even at the simulink level

CC @lrapetti @GiulioRomualdi @gabrielenava @kouroshD

[DanielePucci]

What I was suggesting above is being implemented in #226. Closing.