Implement FixedBase inverse dynamics in TSID

[GiulioRomualdi]

In this issue, we can discuss the implementation of the FixedBase inverse dynamics in TSID component

@isorrentino

[GiulioRomualdi]

The IK implementation can guide you in the software infrastructure

[isorrentino]

I was proceeding with the implementation and I noticed that we have two different implementations of the Task class (one in IK https://github.com/dic-iit/bipedal-locomotion-framework/blob/master/src/IK/include/BipedalLocomotion/IK/LinearTask.h and one in TSID https://github.com/dic-iit/bipedal-locomotion-framework/blob/master/src/TSID/include/BipedalLocomotion/TSID/Task.h) with a different interface. Which one should I use?

cc @GiulioRomualdi @S-Dafarra @traversaro

[traversaro]

I guess TSID, taking inspiration (but not directly using) the Task stuff in IK.

[GiulioRomualdi]

LinearTask is the evolution of Task. In the future, we may think to have only one LinearTask class that is shared between IK and TSID. However, I don't know where we may put that class. (inside Systems?)

[GiulioRomualdi]

LinearTask is the evolution of Task. In the future, we may think to have only one LinearTask class that is shared between IK and TSID. However, I don't know where we may put that class. (inside Systems?)

The future is now #240

[isorrentino]

As done for the IK I implemented an interface for the Task Space Inverse Dynamics and a specific implementation of the interface that considers the robot with a fixed base. The task space problem that we need to solve to find the solution is built as a qp problem. The solution is composed as a vector that contains the base acceleration, joint acceleration, joint torques and contact wrenches.
This means that I defined two tasks in the QPFixedBaseTSID which are:

• a SE3Task on the robot base with the kp and kd gains set to 0
• a JoinDynamicTask to consider the joint dynamics

Here an example for using the QPFixedBaseTSID class:

An application that uses this class can add a task in SE(3) on the end-effector and a task on the joint tracking to be used as regularization.

The commits are:

• isorrentino@991537f for the QPFixedBaseTSID
• isorrentino@04dea6d for the test of the QPFixedBaseTSID class

I still need to complete the documentation. I open the PR as soon as it will be finished.

[GiulioRomualdi]

Notice that the task level proportional block should be substituted with proportional derivative block. Also that one is implemented in lie-groups-controllers

[isorrentino]

The scheme updated:

[traversaro]

By looking at this scheme, I wonder if indeed one of the things I would be curious to test is to use instead of the joint measurements for the "asterisk trick" feedback, use a joint position used by integrating the dynamic model with the constraint of the existing contact as active. I wonder if this would change the performances.

[isorrentino]

The tests fail on MacOS (see https://github.com/dic-iit/bipedal-locomotion-framework/pull/251/checks?check_run_id=2278741869). To understand what is going on I'm using https://github.com/fastai/fastmac/ which gives the possibility to have a MacOS shell.

The problem is that the session expires after some hours. To avoid installing the superbuild from source everytime (it takes almost one hour), I tried to use conda as explained in https://github.com/robotology/robotology-superbuild/blob/master/doc/conda-forge.md by following instructions in https://github.com/robotology/robotology-superbuild/blob/master/doc/install-miniforge.md#macos to install miniforge.

But as soon as I try to create the variable robotologyenv, I get this error:

Mac-1617866944462:fastmac runner$ conda create -n robotologyenv
Collecting package metadata (current_repodata.json): done
Solving environment: done

NotWritableError: The current user does not have write permissions to a required path.
  path: /usr/local/miniconda/pkgs/cache/53d7ef51.json
  uid: 501
  gid: 20

If you feel that permissions on this path are set incorrectly, you can manually
change them by executing

  $ sudo chown 501:20 /usr/local/miniconda/pkgs/cache/53d7ef51.json

In general, it's not advisable to use 'sudo conda'.

miniforge is installed in this location PREFIX=/Users/runner/miniforge3.

@traversaro do you know how to solve this problem?

cc @GiulioRomualdi

[GiulioRomualdi]

Probably conda is already installed in /usr/local/miniconda/

Mac-1617870080053:~ runner$ which conda
/usr/local/miniconda/bin/conda

Yes conda is already installed. I've just tried

sudo conda create -n robotologyenv

[isorrentino]

Yes, you are right. Thanks @GiulioRomualdi

[GiulioRomualdi]

However

robotologyenv) Mac-1617870837768:fastmac runner$ sudo conda install -c robotology yarp
Collecting package metadata (current_repodata.json): done
Solving environment: failed with initial frozen solve. Retrying with flexible solve.
Solving environment: failed with repodata from current_repodata.json, will retry with next repodata s
ource.
Collecting package metadata (repodata.json): done
Solving environment: failed with initial frozen solve. Retrying with flexible solve.
Solving environment: | 
Found conflicts! Looking for incompatible packages.
This can take several minutes.  Press CTRL-C to abort.
failed                                                                                               

UnsatisfiableError: The following specifications were found to be incompatible with each other:

Output in format: Requested package -> Available versions

Package libcxx conflicts for:
python=3.8 -> libcxx[version='>=10.0.0|>=4.0.1']
yarp -> libcxx[version='>=11.1.0']The following specifications were found to be incompatible with you
r system:

  - feature:/osx-64::__osx==10.15.7=0
  - yarp -> __osx[version='>=10.9']

Your installed version is: 10.15.7

[isorrentino]

Yes, I just had the same problem.

[GiulioRomualdi]

Try

conda install cmake compilers make ninja pkg-config

[traversaro]

Conda is already installed, but it is a conda distribution (miniconda, in particular) that differently from miniforge not use by default conda-forge, so you should install also specifying the conda-forge channel:

conda install -c conda-forge -c robotology yarp

However, a possible strategy is just to delete the miniconda installed in GitHub Actions ( conda init --reverse , sudo rm /usr/local/miniconda/, rm -rf ~/.conda, rm -rf ~/.condarc, see https://github.com/robotology/robotology-superbuild/blob/master/doc/install-miniforge.md#uninstall-1) and then install miniforge3 as usual.

[GiulioRomualdi]

Here https://github.com/GiulioRomualdi/fastmac you have a working fastmac action that installs all the dependencies of blf. You should call

conda activate robotologyenv

before compiling blf. Blf is already cloned in the home

The default interactive shell is now zsh.
To update your account to use zsh, please run `chsh -s /bin/zsh`.
For more details, please visit https://support.apple.com/kb/HT208050.
Mac-1617873140301:fastmac runner$ ls
LICENSE         README.md       script-linux.sh script-mac.sh
Mac-1617873140301:fastmac runner$ cd
Mac-1617873140301:~ runner$ ls
Desktop                         Music                           miniforge3
Documents                       Pictures                        runners
Downloads                       bipedal-locomotion-framework    systeminfo.md
Library                         hostedtoolcache                 systeminfo.txt
Miniforge3.sh                   imagedata.json                  work
Movies                          manif
Mac-1617873140301:~ runner$ cd bipedal-locomotion-framework/
Mac-1617873140301:bipedal-locomotion-framework runner$ ls
CHANGELOG.md    LICENSE         README.md       cmake           src
CMakeLists.txt  Python          bindings        devices         utilities
Mac-1617873140301:bipedal-locomotion-framework runner$ conda activate robotologyenv
(robotologyenv) Mac-1617873140301:bipedal-locomotion-framework runner$ 
(robotologyenv) Mac-1617873140301:bipedal-locomotion-framework runner$ ls
CHANGELOG.md    LICENSE         README.md       cmake           src
CMakeLists.txt  Python          bindings        devices         utilities
(robotologyenv) Mac-1617873140301:bipedal-locomotion-framework runner$ mkdir build
(robotologyenv) Mac-1617873140301:bipedal-locomotion-framework runner$ cd build/
(robotologyenv) Mac-1617873140301:build runner$ ccmake ../
(robotologyenv) Mac-1617873140301:build runner$ make 
Scanning dependencies of target TextLogging
[  1%] Building CXX object src/TextLogging/CMakeFiles/TextLogging.dir/src/Logger.cpp.o
[  2%] Linking CXX shared library ../../lib/libBipedalLocomotionFrameworkTextLogging.dylib
ld: warning: -pie being ignored. It is only used when linking a main executable
[  2%] Built target TextLogging
Scanning dependencies of target ParametersHandler
[  3%] Building CXX object src/ParametersHandler/CMakeFiles/ParametersHandler.dir/src/StdImplementation.cpp.o
[  5%] Linking CXX shared library ../../lib/libBipedalLocomotionFrameworkParametersHandler.dylib
ld: warning: -pie being ignored. It is only used when linking a main executable
[  5%] Built target ParametersHandler
Scanning dependencies of target Math
[  6%] Building CXX object src/Math/CMakeFiles/Math.dir/src/CARE.cpp.o

[isorrentino]

I noticed that on MacOS if I change the number of joins the solution is found even using getRandomModel(). Instead, with 20 joints it is not able to find the solution and this is why the tests of the PR are failing. The generated model is:

Joints: 
    [0] link0joint (dofs: 1) : baseLink<-->link0
    [1] link1joint (dofs: 1) : baseLink<-->link1
    [2] link2joint (dofs: 1) : baseLink<-->link2
    [3] link3joint (dofs: 0) : link0<-->link3
    [4] link4joint (dofs: 1) : link0<-->link4
    [5] link5joint (dofs: 1) : link0<-->link5
    [6] link6joint (dofs: 0) : link4<-->link6
    [7] link7joint (dofs: 1) : link3<-->link7
    [8] link8joint (dofs: 1) : link4<-->link8
    [9] link9joint (dofs: 1) : link1<-->link9
    [10] link10joint (dofs: 1) : link4<-->link10
    [11] link11joint (dofs: 0) : link4<-->link11
    [12] link12joint (dofs: 1) : link5<-->link12
    [13] link13joint (dofs: 0) : link7<-->link13
    [14] link14joint (dofs: 0) : link7<-->link14
    [15] link15joint (dofs: 0) : link7<-->link15
    [16] link16joint (dofs: 1) : link5<-->link16
    [17] link17joint (dofs: 0) : link14<-->link17
    [18] link18joint (dofs: 1) : link13<-->link18
    [19] link19joint (dofs: 0) : link3<-->link19

Instead, if I use getRandomChain() with 20 joints, the test fails on Ubuntu and not on MacOS. The success of the tests depends strictly on the generated model and the generated model depends strictly on the operating system.

If I use 15 and 30 joints for the models to test, the tests pass with getRandomChain(). I would push this configuration.

@GiulioRomualdi @traversaro

[traversaro]

The success of the tests depends strictly on the generated model and the generated model depends strictly on the operating system.

And the state of the random number generator. You can change that via srand. However, the fact that getRandomChain fails as well is fishy.

[isorrentino]

The biasForces seem very big when the test fails (20 joints on Linux with getRandomChain() case). I printed the values of the BiasForces and of the joint velocities.

• First iteration

Joint velocity
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
Bias Forces
-8.52651e-14
-1.98952e-13
     680.315
    -3598.33
     1950.45
-1.34029e-12
     2870.79
     371.094
    -24.7526
     348.242
     239.625
     182.833
     99.8967
    -578.137
     509.169
    -725.274
    -2.87886
     729.133
    -114.223
    -285.478
    -151.939
     176.208
     71.0363
      235.32
    -191.872
    -132.905
     -19.284

• Second iteration:

Joint velocity
0.0637869
 0.183229
 0.174338
 0.639822
 0.769366
 -1.63128
 -1.08572
 0.873933
 0.323918
-0.350103
 -2.58609
 0.477722
 0.800899
-0.802584
  3.98979
  1.15025
  3.74264
-0.847134
  27.8719
 -42.0485
  0.38164
Bias Forces
-8720.71
-903.906
   10509
-40091.7
 10595.3
-26824.4
 33769.6
 2222.75
-16986.4
 8954.12
 18062.9
 19446.4
 45483.5
 43562.6
-23503.9
-16667.6
-21735.1
 78974.5
-25859.9
  2804.6
-45986.3
-2558.61
 34644.9
 39875.6
-17347.3
-15283.6
-13979.9

• Third iteration

Joint velocity
-0.0938978
  0.401438
    1.2835
   1.66518
   1.43399
   -4.8092
  -3.97264
  0.963314
   1.39304
  -1.17875
  -6.15239
  0.228348
   2.43107
  -2.40644
   11.0619
   2.89761
   9.21496
  -3.38246
   72.3535
  -108.668
  0.996793
Bias Forces
-48344.6
-2441.78
 63366.1
 -234232
 64641.8
 -144307
  191991
  3191.1
 -102684
 51999.2
  105834
  110684
  263591
  240957
 -145047
 -121272
 -132118
  497282
 -160099
 11971.2
 -271608
-14617.9
  202760
  252383
 -115454
 -149334
  -81609

• 4th iteration

Joint velocity
 -4.45592
  1.78241
  5.96225
  1.80475
  1.05997
 -6.76817
 -9.54561
 -5.86854
  4.79602
-0.284985
 -3.18876
 -8.45892
  4.90179
-0.648779
  18.1343
  0.36033
   3.7494
 -7.67634
  74.0851
 -109.859
  1.28626
Bias Forces
 22180.1
   -6525
 52613.8
 -184697
 97030.7
  243447
 13656.5
 -104427
 -139618
  -45056
 10554.4
 12511.2
 65712.1
-12142.9
 -120669
 -174761
-70033.8
  307937
 -122299
-63096.9
 -145456
 79042.3
  105262
  187583
 -151806
 -200996
 3332.31

• 27th iteration (when it fails)

Joint velocity
-167.748
 339.467
-59.2313
 19.0055
  28.101
-13.1432
 364.693
 120.864
-365.075
-203.491
-17.8371
-46.9005
-27.1051
-101.548
-79.1009
 7.86757
-356.271
 349.115
-151.456
 132.423
-13.9834
Bias Forces
-3.13969e+07
 2.98259e+07
-1.24408e+06
 1.54677e+08
 2.17651e+08
-1.19688e+08
 5.11999e+07
 2.17373e+08
 1.38471e+08
 2.48828e+08
 3.85875e+08
 2.16013e+07
-3.70982e+08
 1.67107e+08
-8.43082e+07
  3.9118e+07
 1.53165e+08
  9.0612e+07
-1.03307e+07
 9.54452e+07
 2.92874e+06
-2.47781e+07
 2.54737e+07
-2.43816e+07
 5.18857e+07
 5.60063e+07
-1.27469e+07

[traversaro]

Interesting, this sounds like an instability. Perhaps dumping the velocity signals using matio-cpp or yarp-telemetry and plotting them could be interesting.

[isorrentino]

PR #251 merged. Closing.