Scheduling

[jbmouret]

I have been thinking about a super-light API for scheduling graphics, physics, and control. The main constraint is that I would like to keep the main loop in the hand of the user (so, no registering of functors or complex APIs like this).

One idea is to make simple scheduling objects like this:

Scheduler control_scheduler(1000); // 1000 Hz
Scheduler graphics_scheduler(40); // 40Hz
Scheduler physics_scheduler(2000);// 2000 Hz

for (double t = 0; t < 10; t += dt) {
    if(control_scheduler(t)) {
     // execute control
    }
   if (graphics_scheduler(t)) {
    graphics->draw();
   }
}

We can play with lambdas too, but it might be more confusing :

control_scheduler(t, [&]{
   // execute control
});
graphics_scheduler(t, [&]{ graphics->draw();});
//...

Or maybe with a single object:

for (double t = 0; t < 10; t += dt) {
    if(scheduler(t, 1000)) {
     // execute control
    }
   if (scheduler(t, 40)) {
    graphics->draw();
   }
}

[Aneoshun]

Why do you want to keep the main loop in the hand of the user?

[jbmouret]

Mainly because the simulator is designed to be incorporated in frameworks that already have their own main loop/flow. When you use a library, you expect to keep the control and not disturb your architecture (this is a main difference between a library and a framework). For instance, the LAAS has a 'compute graph' that connects elements (controller, simulator, footstep planner etc.) like in simulink. It is easy to call my_simulator->step() in a node of their graph, but not if we take the main() (simulator->run(10)).

I also think (especially in research) that we always have unexpected things to put in-between the slots. For instance, imagine you have a neural network that modifies your physics that needs to be called at every physics step. If the authors of the simulator think 'classically', they would have the physics automatically called by their own scheduler at a given frequency (since simulators are usually about control, this is usually the only user-defined part). And this would become impossible / hard to do without using the simulator in a way that it was not intended to. Of course, you could plan for this, but you never plan for everything.

An example of a simulator that takes the control loop is (was? I did not try for some time) V-Rep: each robot embeds its controller. This is nice when you write a small control loop in Python, but terrible when your controller is learning or set by an external evolutionary algorithm, etc.

[costashatz]

The main constraint is that I would like to keep the main loop in the hand of the user

We have discussed this privately many times. I believe that we should provide both alternatives:

• Complete disentanglement of the process: what you describe
• More normal use-case, where the user specifies their controller/descriptors and they do simu.run(20.)

make simple scheduling

Do we really need an object for this? This is just t/dt == freq (not exactly, but you get my point). We can just make a macro for easier usage, no?

[jbmouret]

The object is interesting in the first case because it decouples the frequency setting from the test. If we use the second approach, this is simply a helper function that links the frequency and the t.

[Aneoshun]

I guess you need something more sophisticated than just a condition check if you want to maintain a constant FPS with graphics.

[costashatz]

I guess you need something more sophisticated than just a condition check if you want to maintain a constant FPS with graphics.

I think @jbmouret does not mean real-time FPS or anything. Just in term/with regards to the simulation time, right?

[jbmouret]

I do not think we want to maintain constant FPS like in games. We do not want to have a real-time rendering. The idea is simply that there is no need to update the graphics too often since the screen cannot display more than 60-100Hz anyway. In real robots we can also have a 100 Hz control loop and physics at 1000Hz.

It is true that we could aim at showing the physics 'realistically' by slowing down / accelerating so that what we see is as close as possible to the real time... But I do not think we should spend our time on this because in many robots (like humanoids) we are slower than real-time, not faster (because of the very small dt of physics and the slow QP solvers). => are you interested in slowing-down so that we see the simulation 'at the right speed'? if so, this is a bit more complicated (something like RosRate could work).

In addition, I think a simple way to create a properly timed vizualization is to generate the video at the right FPS (which we do). This makes it possible to pause, replay, vizualize in slow motion, etc.

[Aneoshun]

It's more that now the number of FPS depends on the complexity of the scene and you might have the feeling that your robot is slower, while actually it is just that the scene is more complex. Same goes with the speed of the computer (or worst with hardware acceleration...).
I was simply thinking about a usleep that serves as buffer for a given (maximal) frequency. If the system cannot cope with the requested frequency, then we display a error message. If I recall correctly, this was done like this in robdyn.

[jbmouret]

Yes, this is what rosrate does. We can add this to our requirements and think a bit more :)

[costashatz]

I was simply thinking about a usleep that serves as buffer for a given (maximal) frequency

Yes this is what we had in mind also to provide as an option...

Btw, @Aneoshun if you have time, we are under-going a big restructure and you could help (or any of your students). Especially for creating unit-tests: this is also a good way of learning the library for the students.

[Aneoshun]

@costashatz, Yes, I am trying to follow your interesting discussions, but unfortunately, I am already working 15h+ per day and I don't have any more bandwidth for the next couple of weeks, even if I would love to have time to do some coding!!!. However, I mentioned this restructuring to my two PhD students and they are probably watching this as well, and they are both using robot_dart daily, so they will be impacted by the coming changes. I will invite them to contribute where they can.

[jbmouret]

I think I could implement something like this:

Scheduler scheduler(true);// true to sync. to real time, false by default
for (double t = 0; t < 10; t += dt) {
    scheduler.begin();
    if(scheduler(t, 1000)) {
     // execute control
    }
   if (scheduler(t, 40)) {
    graphics->draw();
   }
  scheduler.end();
}

The end() could wait (with sleep) so that the fastest scheduler() call (here 1000) is synchronized (i.e., if there is less than 1ms between begin() and end(), we wait to have 1ms).

As @costashatz said, we can have this kind of idea for a low-level API and keep a run() that implements a standard loop using the same API), e.g. :

run(physics_frequency, control_frequency, graphics_frequency);

... but this means that that we have to register a control function first and that the graphics is the 'basic one' (e.g. if we want to animate a ghost or display the CoM, we need to use the detailed API).

(this is why I think I would rarely use a 'run' function like this).

[costashatz]

that the graphics is the 'basic one' (e.g. if we want to animate a ghost or display the CoM, we need to use the detailed API).

This is not true. I am animating "ghost" robots with the automatic run. Usually these animated robots come from the controller and I am passing the skeleton pointer (of the ghost robots) to the controller and I am updating them accordingly. This is working very nicely and without any real boilerplate code. One can do the same for the descriptors to animate a goal target or something else.

run(physics_frequency, control_frequency, graphics_frequency);

I am not sure about this one. The control frequency should be part of the controller, not part of the simulator. I think that we should put the control frequency in the controllers, not the simulator object. It makes more sense. I still did not find time though to think about a possible re-restructure of the control sub-part of the library (along with the descriptors).

[jbmouret]

that the graphics is the 'basic one' (e.g. if we want to animate a ghost or display the CoM, we need to use the detailed API).

This is not true. I am animating "ghost" robots with the automatic run. Usually these animated robots come from the controller and I am passing the skeleton pointer (of the ghost robots) to the controller and I am updating them accordingly. This is working very nicely and without any real boilerplate code. One can do the same for the descriptors to animate a goal target or something else.

What I mean is that the link between the graphics and the entity to be displayed has to be made somewhere. If the controller 'publishes' a ghost or a target, then this case has to be anticipated in the run() loop (like, the possibility to publish 'points'). But I am sure we will have more case in the future, that might be highly application-dependent, but that require to have more code to link things.

Without this kind of run() loop, we simply do as we wish, without any constraint and any hypothesis about what the user wants to do.

[costashatz]

Without this kind of run() loop, we simply do as we wish, without any constraint and any hypothesis about what the user wants to do.

To this yes I agree. If someone wants to do something very complicated, they should use the detailed API. Which is why we should provide both.

[jbmouret]

Hey

here is an example.

#include <cassert>
#include <chrono>
#include <cmath>
#include <iostream>
#include <thread>

class Scheduler {
public:
    Scheduler(double dt, bool sync = false) : _dt(dt), _sync(sync) {}
    void begin()
    {
        _max_frequency = 0;
        if (_sync)
            _start = std::chrono::high_resolution_clock::now();
    }
    bool operator()(int frequency)
    {
        _max_frequency = std::max(_max_frequency, frequency);
        double period = std::floor((1. / frequency) / _dt);
        assert(period >= 1.); // if not we need a smaller dt!
        if (_step % int(period) == 0)
            return true;
        return false;
    }
    void end()
    {
        _time += _dt;
        _step += 1;
        _do_sync();
    }
    double time() const { return _time; }
    double next_time() const { return _time + _dt; }
    double dt() const { return _dt; }

protected:
    void _do_sync()
    {
        if (_sync) {
            auto expected = std::chrono::nanoseconds(int(1e9 / _max_frequency));
            auto end = std::chrono::high_resolution_clock::now();
            std::chrono::duration<double> diff = end - _start;
            auto adjust = expected - diff;
            if (adjust.count() > 0)
                std::this_thread::sleep_for(adjust);
        }
    }
    double _time = 0;
    double _dt;
    int _step = 0;
    bool _sync;
    int _max_frequency;
    std::chrono::high_resolution_clock::time_point _start;
};

int main()
{
    Scheduler scheduler(1e-3, true);
    while (scheduler.next_time() < 3) {
        scheduler.begin();

        if (scheduler(1000)) {
            std::cout << 1000 << std::endl;
        }

        if (scheduler(50)) {
            std::cout << 50 << std::endl;
        }

        if (scheduler(20)) {
            std::cout << 20 << std::endl;
        }
        scheduler.end();
    }
    return 0;
}

[costashatz]

here is an example.

This is nice yes! You can make a PR for this. This object is completely independent of all other functionalities of robot_dart and it can be useful for several things. Maybe we can add this in the utils sub-namespace?

if (_sync) {

You forgot a _start = std::chrono::high_resolution_clock::now(); in the end of the if :smile: .

_max_frequency = 0;

This one is better to be 1Hz by default, not zero.

[jbmouret]

begin() is called at every timestep, so why would we need this _start =?

[costashatz]

begin() is called at every timestep, so why would we need this _start =?

I didn't see this. I thought begin was being called only once. Btw, maybe we can merge begin() into the call operator, and rename end() to sync() or sleep(). I think it makes it clearer.

[jbmouret]

This should be done in the PR.