Migrate from internal IK to idyntree-inverseKinematics about human-dynamics-estimation HOT 12 CLOSED

Hi @Yeshasvitvs and @traversaro

Take what I say with a grain of salt as you can see the date of this issue is quite old.
In particular, I thought we already moved to iDynTree::inverse-kinematics, but this is clearly not the case.

As @traversaro said, iDynTree::inverse-kinematics (at least in its late-2017 version) optimises for the whole $q = ({}^A H_B, s) \in SE(3) \times \mathbb{R}^{n_{DOF}}$. In the context of the HDE-IK we only care about the "internal" degrees of freedom, i.e. $s$. The base pose can be easily taken from the measurements.

If you try to make a single (is this what @traversaro meant with "global"?) optimisation problem, you have two drawbacks:

• it might be more computational intensive given the high number of variables,
• you need to specify as costs (as the constrains might, and will, be violated) all the link poses that you need to match. Then you are also introducing weights (that can be all 1 of course) to the problem.

So, let's go back to have multiple IKs.
Before thinking about the multithreading issues, let's solve the problem first.
I am cutting a bit on the math, but if needed I can go in more details.

Consider two links, ${}^A H_{L_1}^d$ and ${}^A H_{L_2}^d$, written w.r.t. a global frame $A$. I called them ${}^d$ as they are given. This is what you get from Xsens.
We want to "match" our internal model to the data, i.e. we want $\operatorname{dist}({}^A H_{L_1}^d, {}^A H_{L_1}(s))$ and $\operatorname{dist}({}^A H_{L_2}^d, {}^A H_{L_2}(s))$ to be as small as possible. Note that, given the dependency on $s$, the solution for $L_1$ is not independent from the solution for $L_2$.
Similar to the "global, single" problem, we can frame this as a classic IK problem, on all $s$. The minimisation should probably found a solution for the joint variables connecting $L_1$ to $L_2$. If framed in this way, I am expecting also some numbers in the other variables (if there is not a regularisation on $s$) which is clearly noise. Furthermore, the base might have some influence on the final error (ideally, the solution should be independent.. only a bias in the error, but with numerical algorithms, I am not so sure this does not have an impact).

We can remove the noise on the other $s$ variables, by using a reduced model, i.e. giving a hint to the solver. Given $s_{L_1, L_2}$ the joint variables connecting $L_1$ to $L_2$ you can have a reduced model with only those variables.
Given that the system is still free-floating you are left with the "problem" of the base as before, but the optimisation problem is now smaller.

For the base, I think that you can add a constraint to the problem: given that you have (from Xsens) ${}^A H_B^d$ you can impose ${}^A H_B = {}^A H_B^d$.
Or set that to the identity, and use relative transforms (w.r.t. the base) for the links, i.e. ${}^B H_{L_1}^d = {}^A H_B^{d, -1} {}^A H_{L_1}^d$.

Regarding the multithreading, having a copy of an iDynTree::Model and IK should be fine for the iDynTree part. Still, you need a linear solver that does not have global static un-protected variables. At the time, MUMPS had problems, the other linear solvers not.

PS:
@traversaro a plugin for Safari ? 😘

from human-dynamics-estimation.

For the time being I was thinking of simply creating N ik solvers configured with a reduced model (one for each pair).

from human-dynamics-estimation.

Switching to a global IK problem will still prevent us to use a thread pool.

@traversaro can you please explain the problem in detail. I am not able to follow this comment.

If we used a single big Inverse Kinematics problems with all the relative link position as tasks, that would be resuilting in a single Ipopt problem instance, that cannot be solved using multiple threads (at least with the linear solvers that we use).

from human-dynamics-estimation.

Also, please explain the limitation of

constraint on the base pose to be identity

Warning: comment that is better rendered using a Github latex extension, such as https://github.com/orsharir/github-mathjax or https://github.com/traversaro/github-mathjax-firefox

Assume that you have an absolute frame $A$ (external to the robot), a base link $B$ and some other link $L$. Simplifying to the essential, iDynTree InverseKinematics class search for the robot position $q = ({}^A H_B, s) \in SE(3) \times \mathbb{R}^{n_{DOF}}$ that minimizes some kind of distances w.r.t. to some desired/reference ${}^A H_L^d$ plus some additional constraints and costs, i.e. it solves the following optimization problem

$$ q^d = \underset{q}{\operatorname{argmax}} \operatorname{dist}({}^A H_L^d, {}^A H_L(q)) $$

where $\operatorname{dist}(.,.): SE(3) \times SE(3) \mapsto \mathbb{R}^3$ is a suitable distance function and ${}^A H_L(.) : SE(3) \times \mathbb{R}^{n_{DOF}} \mapsto SE(3)$ is the so-called forward kinematics function.
If you want to actually optimize the relative transform ${}^B H_L$, i.e. you want to solve the optimization problem:
$$
s^d = \underset{s}{\operatorname{argmax}} \operatorname{dist}({}^B H_L^d, {}^B H_L(s))
$$
you can simply add a constraint ${}^A H_B = 1_4$ to the original optimization problem. Clearly this is not ideal as you have a bigger optimization space, but is better than nothing, and quicker then modifying the inverse kinematics class to solve the relative transform IK.

from human-dynamics-estimation.

In the current implementation the human-state-provider is performing a joint-level inverse kinematics for each pair of bodies, right?
One possible way of handle this is to add a new type of tasks (a "relative" pose) to the iDynTree::InverseKinematics. Switching to a global IK problem will still prevent us to use a thread pool.

from human-dynamics-estimation.

Ahhh, with the constraint on the base pose to be identity? I see.

from human-dynamics-estimation.

Yep.. for the time being it seemed to me the easiest way to migrate to idyntree

from human-dynamics-estimation.

Switching to a global IK problem will still prevent us to use a thread pool.

@traversaro can you please explain the problem in detail. I am not able to follow this comment.

For the time being I was thinking of simply creating N ik solvers configured with a reduced model (one for each pair).

@francesco-romano how did you manage with the dummy links that are used to build three 1 DoF revolute joints.

from human-dynamics-estimation.

Switching to a global IK problem will still prevent us to use a thread pool.

@traversaro can you please explain the problem in detail. I am not able to follow this comment.

If we used a single big Inverse Kinematics problems with all the relative link position as tasks, that would be resuilting in a single Ipopt problem instance, that cannot be solved using multiple threads (at least with the linear solvers that we use).

So, the alternative is to use the idea of creating N ik solvers configured with a reduced model (one for each pair) as it was implemented before. Also, please explain the limitation of

constraint on the base pose to be identity

from human-dynamics-estimation.

This task in done and HumanStateProvider device works with multiple link pair ik using idyntree-inverseKinematics. Check this issue for further details.

from human-dynamics-estimation.

@traversaro a plugin for Safari ? 😘

Not available sorry. : (

from human-dynamics-estimation.

It would be easier to port one of the existing plugins if Safari supported the WebExtensions standard draft: https://browserext.github.io/browserext/ .

from human-dynamics-estimation.