Merge pull request #106 from resibots/status_bar

jbmouret · web-flow · commit bf8955cfa5e7 · 2020-10-08T16:47:44.000+02:00
fix the real time factor + improve scheduler + improve status bar
diff --git a/src/examples/iiwa.cpp b/src/examples/iiwa.cpp
@@ -39,6 +39,7 @@ int main()
     simu.add_checkerboard_floor();
     simu.add_robot(global_robot);
     simu.add_robot(ghost);
+    simu.set_text_panel("IIWA simulation");
     simu.run(20.);
 
     global_robot.reset();
diff --git a/src/python/utils.cpp b/src/python/utils.cpp
@@ -13,21 +13,27 @@ namespace robot_dart {
                     py::arg("dt"),
                     py::arg("sync") = false)
 
-                .def("__call__", &Scheduler::operator())
-                .def("schedule", &Scheduler::schedule)
+                .def("__call__", &Scheduler::operator(),
+                    py::arg("frequency"))
+                .def("schedule", &Scheduler::schedule,
+                    py::arg("frequency"))
 
                 .def("step", &Scheduler::step)
 
                 .def("reset", &Scheduler::reset,
                     py::arg("dt"),
                     py::arg("sync") = false,
-                    py::arg("current_time") = 0.)
+                    py::arg("current_time") = 0.,
+                    py::arg("real_time") = 0.)
 
                 .def("set_sync", &Scheduler::set_sync)
                 .def("sync", &Scheduler::sync)
 
                 .def("current_time", &Scheduler::current_time)
                 .def("next_time", &Scheduler::next_time)
+                .def("real_time", &Scheduler::real_time)
+                .def("real_time_factor", &Scheduler::real_time_factor)
+                .def("it_duration", &Scheduler::it_duration)
                 .def("dt", &Scheduler::dt);
         }
     } // namespace python
diff --git a/src/robot_dart/gui/magnum/glfw_application.cpp b/src/robot_dart/gui/magnum/glfw_application.cpp
@@ -33,8 +33,8 @@ namespace robot_dart {
                 /* Initialize DART world */
                 init(simu, Magnum::GL::defaultFramebuffer.viewport().size()[0], Magnum::GL::defaultFramebuffer.viewport().size()[1]);
 
-                /* Loop at 60 Hz max */
-                setSwapInterval(1);
+                /* No VSync */
+                setSwapInterval(0);
 
                 redraw();
             }
diff --git a/src/robot_dart/gui/magnum/gs/camera.cpp b/src/robot_dart/gui/magnum/gs/camera.cpp
@@ -304,9 +304,9 @@ namespace robot_dart {
                                 if (text->text.empty()) // ignore empty strings
                                     continue;
 
-                                Magnum::GL::Mesh mesh;
+                                Magnum::GL::Mesh mesh{Magnum::NoCreate};
                                 Magnum::Range2D rectangle;
-                                std::tie(mesh, rectangle) = Magnum::Text::Renderer2D::render(*debug_data.font, *debug_data.cache, 28.f, text->text, *debug_data.text_vertices, *debug_data.text_indices, Magnum::GL::BufferUsage::StaticDraw, Magnum::Text::Alignment(text->alignment));
+                                std::tie(mesh, rectangle) = Magnum::Text::Renderer2D::render(*debug_data.font, *debug_data.cache, 28.f, text->text, *debug_data.text_vertices, *debug_data.text_indices, Magnum::GL::BufferUsage::DynamicDraw, Magnum::Text::Alignment(text->alignment));
 
                                 auto viewport = Magnum::Vector2{_camera->viewport()};
                                 auto sc = Magnum::Vector2{viewport.max() / 1024.f};
diff --git a/src/robot_dart/robot_dart_simu.cpp b/src/robot_dart/robot_dart_simu.cpp
@@ -282,7 +282,7 @@ namespace robot_dart {
         if (update_control_freq)
             _control_freq = _physics_freq;
 
-        _scheduler.reset(timestep, _scheduler.sync(), _scheduler.current_time());
+        _scheduler.reset(timestep, _scheduler.sync(), _scheduler.current_time(), _scheduler.real_time());
     }
 
     Eigen::Vector3d RobotDARTSimu::gravity() const
@@ -456,11 +456,13 @@ namespace robot_dart {
         out.precision(3);
         double rt = _scheduler.real_time();
 
-        out << std::fixed << "[Simulation: " << _world->getTime()
+        out << std::fixed << "[simulation time: " << _world->getTime()
             << "s ] ["
-            << "Real: " << rt << "s] [";
+            << "wall time: " << rt << "s] [";
         out.precision(1);
-        out << _world->getTime() / rt << "x]";
+        out << _scheduler.real_time_factor() << "x]";
+        out << " [it: " << _scheduler.it_duration() * 1e3 << " ms]";
+        out << (_scheduler.sync() ? " [sync]" : " [no-sync]");
 
         return out.str();
     }
diff --git a/src/robot_dart/scheduler.cpp b/src/robot_dart/scheduler.cpp
@@ -3,8 +3,10 @@
 namespace robot_dart {
     bool Scheduler::schedule(int frequency)
     {
-        if (_max_frequency == -1)
-            _start = clock_t::now();
+        if (_max_frequency == -1) {
+            _start_time = clock_t::now();
+            _last_iteration_time = _start_time;
+        }
 
         _max_frequency = std::max(_max_frequency, frequency);
         double period = std::round((1. / frequency) / _dt);
@@ -18,13 +20,14 @@ namespace robot_dart {
         return false;
     }
 
-    void Scheduler::reset(double dt, bool sync, double current_time)
+    void Scheduler::reset(double dt, bool sync, double current_time, double real_time)
     {
         ROBOT_DART_EXCEPTION_INTERNAL_ASSERT(dt > 0. && "Time-step needs to be bigger than zero.");
 
         _current_time = 0.;
         _real_time = 0.;
         _simu_start_time = current_time;
+        _real_start_time = real_time;
         _current_step = 0;
         _max_frequency = -1;
 
@@ -38,7 +41,9 @@ namespace robot_dart {
         _current_step += 1;
 
         auto end = clock_t::now();
-        std::chrono::duration<double, std::micro> real = end - _start;
+        _it_duration = std::chrono::duration<double, std::micro>(end - _last_iteration_time).count();
+        _last_iteration_time = end;
+        std::chrono::duration<double, std::micro> real = end - _start_time;
         if (_sync) {
             auto expected = std::chrono::microseconds(int(_current_time * 1e6));
             std::chrono::duration<double, std::micro> adjust = expected - real;
@@ -47,7 +52,7 @@ namespace robot_dart {
         }
 
         _real_time = real.count() * 1e-6;
-        return _real_time;
+        return _real_start_time + _real_time;
     }
 
 } // namespace robot_dart
diff --git a/src/robot_dart/scheduler.hpp b/src/robot_dart/scheduler.hpp
@@ -27,26 +27,34 @@ namespace robot_dart {
         /// returns the real-time (in seconds)
         double step();
 
-        void reset(double dt, bool sync = false, double current_time = 0.);
+        void reset(double dt, bool sync = false, double current_time = 0., double real_time = 0.);
 
+        /// synchronize the simulation clock with the wall clock
+        /// (when possible, i.e. when the simulation is faster than real time)
         void set_sync(bool enable) { _sync = enable; }
-        bool sync() { return _sync; }
+        bool sync() const { return _sync; }
 
-        /// current time according to the simulation
+        /// current time according to the simulation (simulation clock)
         double current_time() const { return _simu_start_time + _current_time; }
-        /// next time according to the simulation
+        /// next time according to the simulation (simulation clock)
         double next_time() const { return _simu_start_time + _current_time + _dt; }
-        /// time according to the clock's computer
-        double real_time() const { return _real_time; }
+        /// time according to the clock's computer (wall clock)
+        double real_time() const { return _real_start_time + _real_time; }
+        /// dt used by the simulation (simulation clock)
         double dt() const { return _dt; }
+        // 0.8x => we are simulating at 80% of real time
+        double real_time_factor() const { return _dt / it_duration(); }
+        // time for a single iteration (wall-clock)
+        double it_duration() const { return _it_duration * 1e-6; }
 
     protected:
-        double _current_time = 0., _simu_start_time = 0., _real_time = 0;
+        double _current_time = 0., _simu_start_time = 0., _real_time = 0., _real_start_time = 0., _it_duration = 0.;
         double _dt;
         int _current_step = 0;
         bool _sync;
         int _max_frequency = -1;
-        clock_t::time_point _start;
+        clock_t::time_point _start_time;
+        clock_t::time_point _last_iteration_time;
     };
 } // namespace robot_dart
 

Original file line number	Diff line number	Diff line change
`@@ -33,8 +33,8 @@ namespace robot_dart {`
`33`	`33`	`/* Initialize DART world */`
`34`	`34`	`init(simu, Magnum::GL::defaultFramebuffer.viewport().size()[0], Magnum::GL::defaultFramebuffer.viewport().size()[1]);`
`35`	`35`
`36`		`- /* Loop at 60 Hz max */`
`37`		`- setSwapInterval(1);`
	`36`	`+ /* No VSync */`
	`37`	`+ setSwapInterval(0);`
`38`	`38`
`39`	`39`	`redraw();`
`40`	`40`	`}`