Exactly same robot may return different KinematicsGeometryHash but it should return the same hash

[eisoku9618]

When we compute the hash, we convert floating point value to string with std::fixed and std::setprecision(4), so the hash can be affected by a super small difference like this.

#include <iostream>
#include <iomanip>

int main () {
    std::ostringstream ss;
    ss << std::fixed << std::setprecision(4);
    double a = 0.001249999999;
    double b = 0.001250;
    ss << a << " " << b;
    std::cout << ss.str() << std::endl;
}

-> 0.0012 0.0013

And this kind of small difference can happen based on the robot kinematics configuration like the following code.

env.Remove(robot)
env.Add(robot)
robot.GetKinematicsGeometryHash() # return hash A
env.Remove(robot)
env.Add(robot)
robot.GetKinematicsGeometryHash() # can return hash B, but robot is not changed at all so should return hash A

It is because

1. KinBody::_ComputeInternalInformation() called in RobotBase::_ComputeInternalInformation does
   like this SetDOFValues(robot.GetDOFValues()) operation, and it changes each link's transform slightly from each original transform.
2. In _ComputeConnectedBodiesInformation called just before KinBody::_ComputeInternalInformation(), _tLeftNoOffset, which is used for kinematic hash computation, is already computed based on the original transform.
3. So next time we call RobotBase::_ComputeInternalInformation even when we don't move robot dof value nor the base link transform, the kinematics hash for the connected body can be changed.

cc @kanbouchou @Puttichai @yoshikikanemoto @strmio @kanunikov-denys @rdiankov

[rdiankov]

I think the simplest way to solve the problem is to round to the nearest 4th or 5th decimal digit (controlled by SERIALIZATION_PRECISION). That should be the most robust since the numbers we input into these things usually don't have that many significant digits.

our current implementation is:

template<typename T>
inline T SerializationValue(T f)
{
    return ( f > -1e-4f && f < 1e-4f ) ? static_cast<T>(0) : f; //boost::math::round(10000*f)*0.0001;
}

which is only doing it for near 0.

You can see we were experimenting with round, but somehow it was commented out for reasons forgotten.

[cielavenir]

update: I thought it was related to mujin redmine 5268, but it seems unrelated; applying b1c7433 did not fix the issue

sorry for confusion

[eisoku9618]

Thank you. Changing SERIALIZATION_PRECISION from 4 to 5 solves my case. What @kanbouchou worried about changing the precision was that it might affect badly existing code. I will check what kind of impact the change has.