Align teleoperation code to integration based IK

[yeshasvitirupachuri]

Currently, the teleoperation code base is aligned to work with pairwise IK of HumanStateProvider with some modifications to handle the fake links added to robot model. The updates of integration based IK in HumanStateProvider diverged with respect to the changes done for teleoperation code. After updating the master to HDEv2, we created a new branch feature/teleoperation to align the changes in HumanStateProvider from both integration based IK and teleoperation.

@kouroshD please update the details related to rebasing order for the code alignment

CC @DanielePucci @lrapetti @traversaro @diegoferigo

[DanielePucci]

@Yeshasvitvs thanks a lot for opening the issue. Please @kouroshD try to keep track of your activities with the issue system

[kouroshD]

Thank you @Yeshasvitvs for opening this issue.
Following are the details (@Yeshasvitvs correct me if sth is not correct):

robotology/feature/teleoperation branch is based on three branches: kourosh/teleoperation_branch and lorenzo/feature/integration_based_ik forks on top of yeshiFork/feature/humanstateprovider_multi_thread_ik branch

• git checkout -b robotology/feature/teleoperation 17875ffea

• git checkout yeshi/feature/humanstateprovider_multi_thread_ik

• git checkout -b tmp

• git reset --hard 7e2978

• git checkout robotology/feature/teleoperation

• git rebase tmp

• git branch -d tmp

• git rebase kouroshFork/teleoperation_branch

• git rebase lorenzoFork/feature/integration_based_ik

[lrapetti]

Just a side note, lorenzo/feature/integration_based_ik will go through last fixies and I will soon open the PR on master branch.

[yeshasvitirupachuri]

Actually we did not start with yeshiFork/feature/humanstateprovider_multi_thread_ik exactly. We started with lorenzo/feature/integration_based_ik and created feature/teleoperation at the commit lrapetti@7e2978b which is just before integration based ik implementation. The commits till here are pari-wise ik which is aligned with the kouroshD/teleoperation_branch. Then we rebased feature/teleoperation branch onto kouroshD/teleoperation_branch and resolved the conflicts by considering correct changes in HumanStateProvider from pair-wise Ik and teleoperation work. Later, we rebased feature/teleoperation onto feature/integration_based_ik and resolved conflicts to update HumanStateProvider with integration based ik implementation.

Summary:

• git checkout -t lrapettifork/feature/integration_based_ik
• git reset --hard lrapetti@7e2978b
• git checkout -b feature/teleoperation
• git push -u origin feature/teleoperation
• git rebase kouroshFork/teleoperation_branch
• git rebase lorenzoFork/feature/integration_based_ik

[kouroshD]

Thank you @Yeshasvitvs for clarification.
Actually, I wanted to write the same git commands as you have mentioned (at the begining), but based on our figure, I thought we stated the robotology/feature/teleoperation based on this commit 17875ffea.
In any case, the first three lines of the git commands you mentioned is equal to set of commands I have mentioned till this line git branch -d tmp, but off course more efficient!

[kouroshD]

Just a side note, lorenzo/feature/integration_based_ik will go through last fixies and I will soon open the PR on master branch.

I would suggest, when you finish the last fixes, I would finish the changes on origin/feature/teleoperation as well. You open the PR on this branch, and we test it, then we open the PR to master. The reason is, there are a number of important fixes and features as well in origin/feature/teleoperation branch, that it can be useful to have it in master branch. Moreover, for the teleoperation experiment, we will use the master branch as well.

[DanielePucci]

@lrapetti

Just a side note, lorenzo/feature/integration_based_ik will go through last fixies and I will soon open the PR on master branch.

Let us know when this is done. We shall clean the overall mess asap.

[kouroshD]

@lrapetti Can you please update this issue, when you are done with rebasing and you could build the repo successfully.

[lrapetti]

The branch in the opened PR is thefeature/integration_based_ik rebased on the feature/teleoperation. All the conflicts have been fixed and it compiles correctly. Furthermore, it has been tested with different options of IK, results will follow.

Before merging the PR I still have to:

• clean the configurations files
• rebase on top of the current master (commits from 399a88f on have not been rebased yet since they require the wearables to be in an unmerged branch, and it would require a different format for the datasets so we can not use previously aquired datasets to test)
• do a general cleaning

[lrapetti]

Following the previous comment, those are the results of the inverse kinematics.

Human

Walking dataset, inverse kinematics with the human model with 48 and 66 DoFs.

• pairwised [48-66 DoFs]
  
• global [48-66 DoFs]
  
• integrationbased [48-66 DoFs]
  

With the human model, all the 3 methods seem to work quite fine both with 48 and 66 DoFs models.
The time to get those solutions however can change a lot depending on the IK solver used. Approximatively, those are the computation times:

solver	t 48DoFs [ms]	t 66DoFs [ms]
pairwised	110	150
global	150	250
integrationbased	16	20

iCub retargeting

When instead I try to use the icub model, with the same walking dataset, the results are strange for both the global and the integratin based IK;

• pairwised
  
• global
  
• integrationbased
  

(The legs of the iCub model are always slightly opened because the urdf was modified in the past in order to avoid hitting the pole during experiments with the real robot)

[lrapetti]

One thing to notice to complete previous comment, with a different dataset (not walking) the global ik solver with the icub model, is working nicely. So there is something strange going on to be investigated.

[lrapetti]

Before merging the PR I still have to:

• clean the configurations files

Done (#106 (comment))

[kouroshD]

We Also, tested the pairWiseIk, in @lrapetti rebased branch (which has an open PR to master branch of robotology/master) with XsensTeloperation and walkingController modules, fixed the small issues and differences, and working same as robotology/feature/teleoperation branch. We ran it for a couple of minutes on simulation.
Result:

[lrapetti]

When instead I try to use the icub model, with the same walking dataset, the results are strange for both the global and the integratin based IK;

• integrationbased
  

The problem has been fixed in 7080d2a.
Now also the integration based IK works properly with the icub model:

The problem was related to the base frame for the KinDyn computation which was never set (now add in 7080d2a#diff-af5a41cc9a872c1dd7da746d7ad6d0f7R587).

@kouroshD @Yeshasvitvs I think we can close this issue, since the code is working and the PR is opened (#106)

[traversaro]

The problem was related to the base frame for the KinDyn computation which was never set (now add in 7080d2a#diff-af5a41cc9a872c1dd7da746d7ad6d0f7R587).

Pay attention that setFloatingBase only supports taking a link as an input, not an arbitrary additinal frame as input, so I suggest to check its output, otherwise given that the parameter name is floatingBaseFrame a user may pass a "frame" to it. By the way, why did you need to change the default base link?

[lrapetti]

@traversaro

Pay attention that setFloatingBase only supports taking a link as an input, not an arbitrary additinal frame as input

If I am not wrong, that's why @kouroshD added fake links (with very small mass and inertia) to the model teleoperation_iCub_model_V_2_5.urdf instead of using frames (and this is the case also for the link root_link_fake that we use as baseframe).

By the way, why did you need to change the default base link?

The reason here is that from the Xsens we get the measurement (position and velocity) from this root_link_fake and not for the root_link, and with the current solution (using a modified model) we are agnostic to the position/orientation of the fake links w.r.t. the real links of the robot.

[traversaro]

Ack, got it. I suspect this then can create problem if we swich to use actual additional frames, the proper solution would be probably to fix robotology/idyntree#422 .

[yeshasvitirupachuri]

The code base is correctly aligned and tested. Closing this issue @lrapetti @kouroshD