From 2f4ba4d54993f78b1d6fbc50df860723f1e9babe Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sat, 26 Sep 2020 20:48:32 +0200 Subject: [PATCH 1/2] Avoid to use std::to_string to pass floating point simulation parameters std::to_string truncates the values with 1e-6 precision, and is prone to locale-related bug. Fix https://github.com/robotology/gym-ignition/issues/245 --- cpp/scenario/gazebo/src/helpers.cpp | 28 +++++++++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/cpp/scenario/gazebo/src/helpers.cpp b/cpp/scenario/gazebo/src/helpers.cpp index 0bfe6eee0..c5fd5a901 100644 --- a/cpp/scenario/gazebo/src/helpers.cpp +++ b/cpp/scenario/gazebo/src/helpers.cpp @@ -40,6 +40,7 @@ #include #include +#include using namespace scenario::gazebo; @@ -335,6 +336,27 @@ bool utils::renameSDFModel(sdf::Root& sdfRoot, return true; } +namespace scenario::gazebo::utils { + /** + * Convert a double to a string ignoring the current locale. + * + * Furthermore, set it with precision 25 to ensure that the + * string can be converted back exactly to the double that + * generated it. + */ + std::string toExactStringNoLocale(const double in); +} + + +std::string utils::toExactStringNoLocale(const double in) +{ + std::ostringstream ss; + ss.imbue(std::locale::classic()); + ss << std::setprecision(25) << in; + return ss.str(); +} + + bool utils::updateSDFPhysics(sdf::Root& sdfRoot, const double maxStepSize, const double rtf, @@ -372,16 +394,16 @@ bool utils::updateSDFPhysics(sdf::Root& sdfRoot, assert(physicsElement); sdf::ElementPtr max_step_size = physicsElement->GetElement("max_step_size"); - max_step_size->AddValue("double", std::to_string(maxStepSize), true); + max_step_size->AddValue("double", toExactStringNoLocale(maxStepSize), true); sdf::ElementPtr real_time_update_rate = physicsElement->GetElement("real_time_update_rate"); real_time_update_rate->AddValue( - "double", std::to_string(realTimeUpdateRate), true); + "double", toExactStringNoLocale(realTimeUpdateRate), true); sdf::ElementPtr real_time_factor = physicsElement->GetElement("real_time_factor"); - real_time_factor->AddValue("double", std::to_string(rtf), true); + real_time_factor->AddValue("double", toExactStringNoLocale(rtf), true); return true; } From 07d068698c70ced86bf48c5ba8ba9896e12a2a9c Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sun, 27 Sep 2020 11:39:10 +0200 Subject: [PATCH 2/2] Add test with 1e-9 timestep --- tests/test_scenario/test_world.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_scenario/test_world.py b/tests/test_scenario/test_world.py index 83d2ee80e..a72b70682 100644 --- a/tests/test_scenario/test_world.py +++ b/tests/test_scenario/test_world.py @@ -183,7 +183,7 @@ def test_world_physics_plugin(gazebo: scenario.GazeboSimulator): @pytest.mark.parametrize("gazebo", - [(0.001, 1.0, 1)], + [(0.001, 1.0, 1), (1e-9, 1.0, 1)], indirect=True, ids=utils.id_gazebo_fn) def test_sim_time_starts_from_zero(gazebo: scenario.GazeboSimulator): @@ -207,4 +207,4 @@ def test_sim_time_starts_from_zero(gazebo: scenario.GazeboSimulator): assert world.time() == 2 * dt gazebo.run(paused=False) - assert world.time() == 3 * dt + assert world.time() == pytest.approx(3 * dt)