Create an example to test the HumanIK class

[davidegorbani]

I open this issue in order to create a C++ executable that reads data from a .mat of an experiment and performs the inverse kinematic using the data from the nodes.

[davidegorbani]

I created the executable with 97d1013; the .mat files saved on the storage to be loaded by the application they have to be saved with MATLAB version 7.3.
However, at the first iteration the solver fails with the following output:

[2023-12-13 17:32:56.501] [thread: 54059] [blf] [error] [QPInverseKinematics::advance] osqp was not able to find a feasible solution.
[OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets] The eigenSparseMatrix is empty.
[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to evaluate triplets from the old hessian matrix.
[2023-12-14 12:24:27.020] [thread: 46797] [blf] [error] [QPInversekinematics::Impl::updateSolver] Unable to set the constraint matrix.
[2023-12-14 12:24:27.020] [thread: 46797] [blf] [error] [QPInverseKinematics::advance] Unable to update the QP solver.

[davidegorbani]

The solver couldn't find a solution because I set the priority of all the task to highPriority, switching to lowPriority it was able to find the solution. However, setting all the task to lowPriority I get the following error:

[OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets] The eigenSparseMatrix is empty.
[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to evaluate triplets from the old hessian matrix.
[2023-12-14 12:24:27.020] [thread: 46797] [blf] [error] [QPInversekinematics::Impl::updateSolver] Unable to set the constraint matrix.
[2023-12-14 12:24:27.020] [thread: 46797] [blf] [error] [QPInverseKinematics::advance] Unable to update the QP solver.

This is a know problem related to blf and ami-iit/bipedal-locomotion-framework#723 is open to address it.

[davidegorbani]

I did a test of the IK implemented in HumanIK using a dataset saved in a .mat file and compared the results with the expected results; here is the video of the comparison:

Screencast.from.12-01-2024.16.32.03.webm

The expected result is on the left, while the result of the IK is on the right; as it is possible to notice, the model on the right is rotated of about 180 degrees w.r.t. the model on the left; moreover, the legs have a different behaviour which could be related to the fact that the gravity tasks haven't been added yet.

[davidegorbani]

Together with @dariosortino and @marcoforleo, today we acquire a dataset before performing the T pose calibration to check if starting from the same raw measurement the IK implemented in HDE and the IK implemented in HumanIK class give the same results; as it is possible to notice from the following recording, they are very similar (on the left the HumanIK IK and on the right the HDE IK):

Screencast.from.02-02-2024.16.13.10.webm

Then we acquired a dataset while performing the T pose calibration; I implemented a function in HumanIK to perform the T pose calibration (commit 3802491) and I ran this function in the example application at the same moment in which it was performed with HDE; the following video shows the comparison of the two IK:

Screencast.from.02-02-2024.16.27.35.webm

Thanks @Gianlucamilani @dariosortino and @marcoforleo for your help acquiring data!

[DanielePucci]

Awesome work @davidegorbani!

CC
@ami-iit/ifeel @claudia-lat

[davidegorbani]

Closing this issue since I merged #19.