Distance and gravity tasks #717

S-Dafarra · 2023-08-01T15:15:35Z

Implements the DistanceTask and GravityTask initially implemented by @EhsanRanjbari

S-Dafarra · 2023-08-02T09:59:53Z

CI seemed happy, I cleaned the history

S-Dafarra · 2023-08-03T11:11:40Z

I realized there might have been a mistake in the GravityTask. See #651 (comment)

The tests in the current form do not fail in either case, so I tried to switch back the tasks to constraint in a3baa19, but it still fails on CI (while on my PC it was working better). With 954b608 I moved them back to cost.

S-Dafarra · 2023-08-08T09:14:56Z

The tests in the current form do not fail in either case, so I tried to switch back the tasks to constraint in a3baa19, but it still fails on CI (while on my PC it was working better). With 954b608 I moved them back to cost.

I dug further into this and realized that the gravity error was always zero independently from how big the modification to the initial joint configuration was. Then, I realized two things:

the modified initial state was not set to the kindyn object. Fixed in e3cb598. See

bipedal-locomotion-framework/src/IK/tests/QPInverseKinematicsTest.cpp

Lines 308 to 309 in e3cb598

REQUIRE(

kinDyn->setRobotState(basePose, jointPositions, baseVelocity, jointVelocities, gravity));

the random models generated by iDynTree seem only planar. Hence, by only modifying the joint values, the gravity measured in a frame was never changing. I fixed it by adding a small perturbation to the initial base rotation. See

bipedal-locomotion-framework/src/IK/tests/QPInverseKinematicsTest.cpp

Lines 294 to 302 in e3cb598

    
           Eigen::AngleAxisd baseAngleAxis(basePose.topLeftCorner<3, 3>()); 
        
           double newAngle = baseAngleAxis.angle() + distribution(generator); 
        
           Eigen::AngleAxisd newBaseRotation(newAngle, baseAngleAxis.axis()); 
        
           out.dynamics = std::make_shared<FloatingBaseSystemKinematics>(); 
        
           out.dynamics->setState( 
        
               {basePose.topRightCorner<3, 1>(), toManifRot(newBaseRotation.matrix()), jointPositions});

Now, when changing the sign in the gravity task, the test case fails as expected! Curious to see the output of CI

S-Dafarra · 2023-08-08T10:09:56Z

CI is happy 🎉 I am going to clean again the history

src/IK/src/GravityTask.cpp

EhsanRanjbari

LGTM!

S-Dafarra · 2023-09-25T09:03:28Z

@GiulioRomualdi friendly ping.

This code has been tested also by @EhsanRanjbari in his application

GiulioRomualdi · 2023-09-25T09:40:26Z

I will check the PR within today. I'm sorry for the late response

CHANGELOG.md

src/IK/include/BipedalLocomotion/IK/DistanceTask.h

src/IK/include/BipedalLocomotion/IK/GravityTask.h

src/IK/include/BipedalLocomotion/IK/DistanceTask.h

S-Dafarra · 2023-09-26T08:16:05Z

Rebasing on top of master

m_baseFrameName removed resize world_T_framePosition pow(world_T_framePosition(2),2) added made m_computedDistance public member Fixed use of relative distance task DIstanceTask added made m_computedDistance public member gitignore updated DistanceTask PR review: Naming fixed, m_frameName and m_frameEEName removed, unused headers removed.

Using fixed dimension vector for frame position in Distance task The input distance is normalized Applied clang-format to DistanceTask

Added DistanceTask to QPInverseKinematicsTest

GravityTask_Preliminary_Implementation GravityTask_Preliminary_ImplementationRev2 Fixed setEstimateGravityDir method matrices size fixed m_robotVelocityVariable.name fixed trailing whitespace fixed m_angularJacobian fixed, setEstimateGravityDir parameter fixed body to world rotation added feedforward gyro added

Fixed sign in GravityTask Added gravity task documentation. Applied clang-format to GravityTask

Added GravityTask to QPInverseKinematics Fix failure of QPInveseKinematicsTest by moving Gravity task to low priority Moving also distance task to low priority in QPInverseKinematics Fixed a sign mistake in GravityTask. This might have been the reason why the IK was failing before Added explanation on how the gravity task is obtained directly in the code.

It was necessary to refactor a bit the structure of the code, porting in each test case the addition of the tasks we are interested in. Added a small rotation error in the base. This is necessary since the random models generated by iDynTree seem always planar.

S-Dafarra · 2023-09-26T14:21:03Z

For some reason, the CI stays stuck in the dependencies phase on Ubuntu.

S-Dafarra · 2023-09-27T07:34:18Z

For some reason, the CI stays stuck in the dependencies phase on Ubuntu.

Here the output:

Transaction
  Prefix: /usr/share/miniconda3/envs/test
  Updating specs:
   - cmake
   - compilers
   - make
   - ninja
   - pkg-config
  Package                          Version  Build          Channel                   Size
───────────────────────────────────────────────────────────────────────────────────────────
  Install:
───────────────────────────────────────────────────────────────────────────────────────────
  + _libgcc_mutex                      0.1  conda_forge    conda-forge/linux-64       3kB
  + _openmp_mutex                      4.5  2_gnu          conda-forge/linux-64      24kB
  + binutils                          2.40  hdd6e379_0     conda-forge/linux-64      30kB
  + binutils_impl_linux-64            2.40  hf600244_0     conda-forge/linux-64       5MB
  + binutils_linux-64                 2.40  hbdbef99_2     conda-forge/linux-64      28kB
  + bzip2                            1.0.8  h7f98852_4     conda-forge/linux-64     496kB
  + c-ares                          1.19.1  hd590300_0     conda-forge/linux-64     113kB
  + c-compiler                       1.6.0  hd590300_0     conda-forge/linux-64       6kB
  + ca-certificates              2023.7.22  hbcca054_0     conda-forge/linux-64     150kB
  + cmake                           3.27.6  hcfe8598_0     conda-forge/linux-64      [18](https://github.com/ami-iit/bipedal-locomotion-framework/actions/runs/6311199609/job/17168602990#step:4:19)MB
  + compilers                        1.6.0  ha770c72_0     conda-forge/linux-64       7kB
  + cxx-compiler                     1.6.0  h00ab1b0_0     conda-forge/linux-64       6kB
  + fortran-compiler                 1.6.0  heb67821_0     conda-forge/linux-64       6kB
  + gcc                             12.3.0  h8d2909c_2     conda-forge/linux-64      27kB
  + gcc_impl_linux-64               12.3.0  he2b93b0_2     conda-forge/linux-64      52MB
  + gcc_linux-64                    12.3.0  h76fc315_2     conda-forge/linux-64      30kB
  + gfortran                        12.3.0  h499e0f7_2     conda-forge/linux-64      27kB
  + gfortran_impl_linux-64          12.3.0  hfcedea8_2     conda-forge/linux-64      15MB
  + gfortran_linux-64               12.3.0  h7fe76b4_2     conda-forge/linux-64      29kB
  + gxx                             12.3.0  h8d2909c_2     conda-forge/linux-64      27kB
  + gxx_impl_linux-64               12.3.0  he2b93b0_2     conda-forge/linux-64      13MB
  + gxx_linux-64                    12.3.0  h8a814eb_2     conda-forge/linux-64      29kB
  + kernel-headers_linux-64         2.6.32  he073ed8_16    conda-forge/noarch       709kB
  + keyutils                         1.6.1  h166bdaf_0     conda-forge/linux-64     118kB
  + krb5                            1.21.2  h659d440_0     conda-forge/linux-64       1MB
  + ld_impl_linux-64                  2.40  h41732ed_0     conda-forge/linux-64     705kB
  + libcurl                          8.3.0  hca28451_0     conda-forge/linux-64     388kB
  + libedit                   3.1.20[19](https://github.com/ami-iit/bipedal-locomotion-framework/actions/runs/6311199609/job/17168602990#step:4:20)1231  he28a2e2_2     conda-forge/linux-64     124kB
  + libev                             4.33  h516909a_1     conda-forge/linux-64     106kB
  + libexpat                         2.5.0  hcb278e6_1     conda-forge/linux-64      78kB
  + libgcc-devel_linux-64           12.3.0  h8bca6fd_2     conda-forge/linux-64       2MB
  + libgcc-ng                       13.2.0  h807b86a_2     conda-forge/linux-64     771kB
  + libgfortran5                    13.2.0  ha4646dd_2     conda-forge/linux-64       1MB
  + libgomp                         13.2.0  h807b86a_2     conda-forge/linux-64     4[21](https://github.com/ami-iit/bipedal-locomotion-framework/actions/runs/6311199609/job/17168602990#step:4:22)kB
  + libnghttp2                      1.52.0  h61bc06f_0     conda-forge/linux-64     6[22](https://github.com/ami-iit/bipedal-locomotion-framework/actions/runs/6311199609/job/17168602990#step:4:23)kB
  + libsanitizer                    12.3.0  h0f45ef3_2     conda-forge/linux-64       4MB
  + libssh2                         1.11.0  h0841786_0     conda-forge/linux-64     271kB
  + libstdcxx-devel_linux-64        12.3.0  h8bca6fd_2     conda-forge/linux-64       9MB
  + libstdcxx-ng                    13.2.0  h7e041cc_2     conda-forge/linux-64       4MB
  + libuv                           1.46.0  hd590300_0     conda-forge/linux-64     893kB
  + libzlib                         1.2.13  hd590300_5     conda-forge/linux-64      62kB
  + make                               4.3  hd18ef5c_1     conda-forge/linux-64     519kB
  + ncurses                            6.4  hcb278e6_0     conda-forge/linux-64     881kB
  + ninja                           1.11.1  h924138e_0     conda-forge/linux-64       2MB
  + openssl                          3.1.3  hd590300_0     conda-forge/linux-64       3MB
  + pkg-config                      0.29.2  h36c2ea0_1008  conda-forge/linux-64     1[23](https://github.com/ami-iit/bipedal-locomotion-framework/actions/runs/6311199609/job/17168602990#step:4:24)kB
  + rhash                            1.4.4  hd590300_0     conda-forge/linux-64     185kB
  + sysroot_linux-64                  2.12  he073ed8_16    conda-forge/noarch        15MB
  + xz                               5.2.6  h166bdaf_0     conda-forge/linux-64     418kB
  + zstd                             1.5.5  hfc55[25](https://github.com/ami-iit/bipedal-locomotion-framework/actions/runs/6311199609/job/17168602990#step:4:26)1_0     conda-forge/linux-64     545kB
  Summary:
  Install: 50 packages
  Total download: 154MB
───────────────────────────────────────────────────────────────────────────────────────────
Looking for: ['cmake', 'compilers', 'make', 'ninja', 'pkg-config']
Downloading and Extracting Packages
Preparing transaction: ...working... done
Verifying transaction: ...working... done
Executing transaction: ...working... done
conda-forge/linux-[64](https://github.com/ami-iit/bipedal-locomotion-framework/actions/runs/6311199609/job/17168602990#step:4:65)                                        Using cache
conda-forge/noarch                                          Using cache

GiulioRomualdi

Some small comments

GiulioRomualdi · 2023-09-27T09:27:11Z

src/IK/src/DistanceTask.cpp

+
+    toEigen(this->subA(m_robotVelocityVariable))
+        = (m_framePosition.transpose() * m_jacobian.topRows<3>())
+          / (std::max(0.001, m_computedDistance));


Can you remove 0.001 from the code? (I don't know if it makes sense to have it as parameter)

It helps for the stability of the algorithm. I have moved it to an optional input parameter in c47e541

src/IK/src/DistanceTask.cpp

src/IK/src/GravityTask.cpp

GiulioRomualdi · 2023-09-27T09:33:23Z

src/IK/src/GravityTask.cpp

+bool GravityTask::setDesiredGravityDirectionInTargetFrame(
+    const Eigen::Ref<const Eigen::Vector3d> desiredGravityDirection)
+{
+    m_desiredZDirectionBody = desiredGravityDirection;


what if the norm is almost zero?

The normalize method from Eigen should already consider this case: https://github.com/libigl/eigen/blob/1f05f51517ec4fd91eed711e0f89e97a7c028c0e/Eigen/src/Core/Dot.h#L142-L147

Basically, if the norm is too small, it is left unchanged.

GiulioRomualdi · 2023-09-27T09:36:39Z

Regarding the CI problem, it's not related to this PR still is really annoying 😭

Co-authored-by: Giulio Romualdi <giulio.romualdi@gmail.com>

GiulioRomualdi · 2023-09-27T21:01:58Z

Thank you @S-Dafarra and @EhsanRanjbari for the contribution. Merging!

mebbaid · 2023-10-02T08:38:50Z

Regarding the CI problem, it's not related to this PR still is really annoying

I suspect that the CI problem has something to do with devices/YarpRobotLoggerDevice

I say this because when I was modifying some ergoCubGazeboV1 config parameters for the logger as part of this closed PR #703 i had these same CI problem. When I opened the new PR without touching the Logger, I had no CI issues see #734

github-actions bot added 🔨 component: IK 🐍 bindings labels Aug 1, 2023

S-Dafarra force-pushed the distance_and_gravity_tasks branch from c3db224 to 7c88593 Compare August 1, 2023 15:16

S-Dafarra marked this pull request as ready for review August 1, 2023 15:17

S-Dafarra requested a review from GiulioRomualdi as a code owner August 1, 2023 15:17

S-Dafarra requested a review from EhsanRanjbari August 1, 2023 15:17

S-Dafarra self-assigned this Aug 1, 2023

S-Dafarra force-pushed the distance_and_gravity_tasks branch from fdc8f56 to a6078ce Compare August 2, 2023 09:59

S-Dafarra force-pushed the distance_and_gravity_tasks branch from e3cb598 to 700bf61 Compare August 8, 2023 10:14

EhsanRanjbari reviewed Aug 10, 2023

View reviewed changes

src/IK/src/GravityTask.cpp Outdated Show resolved Hide resolved

EhsanRanjbari reviewed Aug 10, 2023

View reviewed changes

src/IK/src/GravityTask.cpp Outdated Show resolved Hide resolved

EhsanRanjbari reviewed Aug 16, 2023

View reviewed changes

src/IK/src/GravityTask.cpp Show resolved Hide resolved

EhsanRanjbari approved these changes Aug 16, 2023

View reviewed changes

GiulioRomualdi reviewed Sep 25, 2023

View reviewed changes

Ehasn and others added 9 commits September 26, 2023 10:16

Small refactory of the DistanceTask and added documentation.

13c4021

Using fixed dimension vector for frame position in Distance task The input distance is normalized Applied clang-format to DistanceTask

Added tests for DistanceTask and bugfix.

73d03b3

Added DistanceTask to QPInverseKinematicsTest

Added DistanceTask python bindings

cd6d53c

Small refactory of GravityTask and added documentation.

0dae6b5

Fixed sign in GravityTask Added gravity task documentation. Applied clang-format to GravityTask

Added GravityTask python bindings

5952b91

S-Dafarra added 4 commits September 26, 2023 10:16

Updated CHANGELOG.

03ed81c

Specified the unit of measurements in the feedforward of gravity task

82de935

Renamed base_frame_name to target_frame_name in the distance task

3521e68

Added setSetPoint method to distance task

8daed97

S-Dafarra force-pushed the distance_and_gravity_tasks branch from 1d17868 to 9e5b03e Compare September 26, 2023 08:18

S-Dafarra added 2 commits September 26, 2023 12:11

Added setSetPoint method to GravityTask

36af50f

Updated Changelog

972ab04

S-Dafarra force-pushed the distance_and_gravity_tasks branch from 9e5b03e to 972ab04 Compare September 26, 2023 10:11

GiulioRomualdi reviewed Sep 27, 2023

View reviewed changes

Added suggestions to improve the distance and gravity task computations

88eb49a

Co-authored-by: Giulio Romualdi <giulio.romualdi@gmail.com>

S-Dafarra force-pushed the distance_and_gravity_tasks branch from 469f4b8 to 88eb49a Compare September 27, 2023 12:16

Moved a magic number to input parameter in DistanceTask

c47e541

GiulioRomualdi merged commit 7723da1 into master Sep 27, 2023
14 of 15 checks passed

GiulioRomualdi deleted the distance_and_gravity_tasks branch September 27, 2023 21:03

GiulioRomualdi mentioned this pull request Nov 2, 2023

Add IK align vector direction task #651

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Distance and gravity tasks #717

Distance and gravity tasks #717

S-Dafarra commented Aug 1, 2023

S-Dafarra commented Aug 2, 2023

S-Dafarra commented Aug 3, 2023

S-Dafarra commented Aug 8, 2023

S-Dafarra commented Aug 8, 2023

EhsanRanjbari left a comment

S-Dafarra commented Sep 25, 2023

GiulioRomualdi commented Sep 25, 2023

S-Dafarra commented Sep 26, 2023

S-Dafarra commented Sep 26, 2023

S-Dafarra commented Sep 27, 2023

GiulioRomualdi left a comment

GiulioRomualdi Sep 27, 2023

S-Dafarra Sep 27, 2023

GiulioRomualdi Sep 27, 2023

S-Dafarra Sep 27, 2023

GiulioRomualdi commented Sep 27, 2023

GiulioRomualdi commented Sep 27, 2023

mebbaid commented Oct 2, 2023

Distance and gravity tasks #717

Distance and gravity tasks #717

Conversation

S-Dafarra commented Aug 1, 2023

S-Dafarra commented Aug 2, 2023

S-Dafarra commented Aug 3, 2023

S-Dafarra commented Aug 8, 2023

S-Dafarra commented Aug 8, 2023

EhsanRanjbari left a comment

Choose a reason for hiding this comment

S-Dafarra commented Sep 25, 2023

GiulioRomualdi commented Sep 25, 2023

S-Dafarra commented Sep 26, 2023

S-Dafarra commented Sep 26, 2023

S-Dafarra commented Sep 27, 2023

GiulioRomualdi left a comment

Choose a reason for hiding this comment

GiulioRomualdi Sep 27, 2023

Choose a reason for hiding this comment

S-Dafarra Sep 27, 2023

Choose a reason for hiding this comment

GiulioRomualdi Sep 27, 2023

Choose a reason for hiding this comment

S-Dafarra Sep 27, 2023

Choose a reason for hiding this comment

GiulioRomualdi commented Sep 27, 2023

GiulioRomualdi commented Sep 27, 2023

mebbaid commented Oct 2, 2023