Restructure ContactDetector machinery

[GiulioRomualdi]

This is the first PR that will bring the CentroidalMPC merged in master.
While implementing the WholeBodyQPBlock, I noticed that the behavior of FixedFootDetector (the class used to get the fixed foot given planned footsteps) was not coherent with the others advanceable in the project. Indeed, before this PR, a user had to call FixedFootDetector::getFixedFoot() before calling FixedFootDetector::advance(). This behavior was counterintuitive since the Advanceable should be able to provide its output only if advance() is called. Moreover, the FixedFootDetector internal timer was reset when a new sequence of contact was provided. This prevented the usage of FixedFootDetector without having a predefined contact sequence.
Thanks to this refactory the FixedFootDetector usage becomes similar to the others
advanceable. Indeed now advance() considers the input set by the user and provides the
corresponding output.
⚠️ Even if this modification does not break the API the user may notice some strange behavior if advance() was called after getting the output of the detector. The only user affected by this modification should be (@GiulioRomualdi in paper_romualdi_2022_icra_centroidal-mpc-walking)

While implementing paper_romualdi_2022_icra_centroidal-mpc-walking), I also noticed some undesired behavior due to the number representation of double. Indeed I noticed that the following code

double time = 0.0
double dt = 0.1;
time += dt;
time += dt;
time += dt;

bool is_lower_equal = time <= 0.3;

was giving me is_lower_equal == False. This is due to round-off error Indeed time was slightly lower than 0.3 (for instance 0.3 - 1e-8). This commit 2b6ada8 robustifies the comparison by adding a tolerance (by default equal to 0) that considers the round-off error.

Last but not least in the refractory I also moved the implementation of the SchmittTrigger into the math component since it can be used also by other components.

[GiulioRomualdi]

Windows is failing with the following error:

D:\a\bipedal-locomotion-framework\bipedal-locomotion-framework\src\Contacts\src\SchmittTriggerDetector.cpp(99): error C7555: use of designated initializers requires at least '/std:c++20'
D:\a\bipedal-locomotion-framework\bipedal-locomotion-framework\src\Contacts\src\SchmittTriggerDetector.cpp(258): error C7555: use of designated initializers requires at least '/std:c++20'
D:\a\bipedal-locomotion-framework\bipedal-locomotion-framework\src\Contacts\src\SchmittTriggerDetector.cpp(274): error C7555: use of designated initializers requires at least '/std:c++20'

I wasn't aware of this. I need to restructure the code

[S-Dafarra]

While implementing paper_romualdi_2022_icra_centroidal-mpc-walking), I also noticed some undesired behavior due to the number representation of double. Indeed I noticed that the following code

double time = 0.0
double dt = 0.1;
time += dt;
time += dt;
time += dt;

bool is_lower_equal time <= 0.3;

was giving me is_lower_equal == False. This is due to round-off error Indeed time was slightly lower than 0.3 (for instance 0.3 - 1e-8). This commit 2b6ada8 robustifies the comparison by adding a tolerance (by default equal to 0) that considers the round-off error.

I thought it would be interesting to add that 0.1 is a periodic number in binary notation. Hence, it is always "approximated". That's why their sum is different from the product. A good explanation can be found here: https://youtu.be/PZRI1IfStY0?t=192

[S-Dafarra]

Some preliminary comments. I still need to check a bunch of files

[S-Dafarra]

What do you mean by "Time units"?

[S-Dafarra]

I think a piece is missing here.

[S-Dafarra]

Why epsilon and not min?

[GiulioRomualdi]

You are right. At this point, I think that epsilon is better than min indeed as explained here

• std::numeric_limits<double>::epsilon() Returns the machine epsilon, that is, the difference between 1.0 and the next value representable by the floating-point type T.

• std::numeric_limits<double>::min() Returns the minimum finite value representable by the numeric type T.

In https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon, shows how to use std::numeric_limits<double>::epsilon() to compare floating-point values for equality.

[GiulioRomualdi]

We may implement something like this in Math component and use it everywhere is needed (Taken from https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon)

template<class T>
typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
    almost_equal(T x, T y, int ulp)
{
    // the machine epsilon has to be scaled to the magnitude of the values used
    // and multiplied by the desired precision in ULPs (units in the last place)
    return std::fabs(x - y) <= std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp
        // unless the result is subnormal
        || std::fabs(x - y) < std::numeric_limits<T>::min();
}

Where ULP is

Unit in the last place (ULP) represents the value 2-23, which is approximately 1.192093e-07. The ULP is the distance between the closest straddling floating point numbers a and b (a ≤ x ≤ b, a ≠ b), assuming that the exponent range is not upper-bounded. The IEEE Round-to-Nearest modes produce results with a maximum error of one-half ULP. The other IEEE rounding modes (Round-to-Zero, Round-to-Positive-Infinity, and Round-to-Negative-Infinity) produce results with a maximum error of one ULP.

Additional ref: https://www.intel.com/content/www/us/en/docs/programmable/683242/current/unit-in-the-last-place.html

[S-Dafarra]

That's where my doubt arose. epsilon seems to work better for checking if you are close to 1, while in

bipedal-locomotion-framework/src/Math/src/SchmittTrigger.cpp

Line 77 in f3241a5

return (std::abs(a - b) <= this->m_params.timeComparisonThreshold);

you are checking a difference that should be close to zero. Note that the example of https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon have another part on the right hand side. In order to copy it you should do

(std::abs(a - b) <= this->m_params.timeComparisonThreshold) * std::abs(a + b) )

but I don't think it is worth adding the additional multiplication. In addition, it makes the check more and more coarse the higher is a+b. That's why they also added the

|| std::fabs(x - y) < std::numeric_limits<T>::min();

in the check (again using min).

In any case, we are talking about an infinitesimal difference, so feel free to use the version you prefer 😊

[S-Dafarra]

We may implement something like this in Math component and use it everywhere is needed (Taken from https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon)

template<class T>
typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
    almost_equal(T x, T y, int ulp)
{
    // the machine epsilon has to be scaled to the magnitude of the values used
    // and multiplied by the desired precision in ULPs (units in the last place)
    return std::fabs(x - y) <= std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp
        // unless the result is subnormal
        || std::fabs(x - y) < std::numeric_limits<T>::min();
}

Loving it, but I guess it is a bit of an over-complexification 🤔

[GiulioRomualdi]

Here I am! So I tried to do some tests following this nice post with a toy problem implemented in floating-point-comparison. Here I implemented for type of comparisons

template <typename T>
bool comparison0(T f1, T f2, T tolerance)
{
    return f1 == f2;
}

template <typename T>
bool comparison1(T f1, T f2, T tolerance)
{
    return std::abs(f1 - f2) <= tolerance;
}

template <typename T>
bool comparison2(T f1, T f2, T tolerance)
{
    return std::abs(f1 - f2) <= tolerance * std::max(std::abs(f1), std::abs(f2));
}

template <typename T>
bool comparison3(T f1, T f2, T tolerance)
{
    std::size_t ulp = 2;

    return std::fabs(f1 - f2) <= tolerance * std::fabs(f1 + f2) * ulp
        || std::fabs(f1 - f2) < std::numeric_limits<T>::min();
}

The equalities are tested for from 9.0 to 99 in steps of 0.01

Setting the tolerance equal to 1e-10 We got the following results where correct means the number of comparisons that were correct

Comparison Function	correct	total
0	10	10000
1	10000	10000
2	9999	10000
3	9999	10000

On the other hand setting the tolerance equal to std::numeric_limit<double>::epsilon() I got the following

Comparison Function	correct	total
0	24	10000
1	42	10000
2	70	10000
3	274	10000

The following conclusion holds (not in general but in our case):

• std::numeric_limit<double>::epsilon() is too low tolerance
• even if here, and here suggested to avoid considering an absolute threshold for us comparison1() seems to be enough.

So we should decide the tolerance we want to guarantee in blf. (We can store it as parameter in Math component along with the Gravity)

@traversaro suggested: https://0.30000000000000004.com/

[S-Dafarra]

Nice! 1e-10 looks reasonable to me!

[S-Dafarra]

Also here I think it is not clear what is meant by time-units

[GiulioRomualdi]

Hi @S-Dafarra, I applied most of the changes you suggested except #624 (comment) since #630 will revisit how the time is handled inside the entire project.

The plan is to merge this PR and then move to #630