Need of using multiple worlds in the same application arise when:

  • There is the need to implement cascading controllers architecture, or more generally when more than one controllers need to exchange data.
  • The controllers involved do not run at same frequency.
  • Their execution in the same thread is not possible because even if dealing with their period synchronization correctly, executing them would break real time requirements of at least one of them.

More generally, even if the 2 last hypothesis are not verified, it is probably most of time simpler to think of controllers as executed in separated threads, because their individual implementation is simpler than a global mono thread implementation dealing with their execution frequency and delay in the good way (if there are more than 2 controllers).

So dealing with multiple worlds is basically about dealing with multiple controllers in a mutli-thread application.

Interacting controllers

Cascading controllers architectures are puting in place interacting controllers: higher level controllers update commands while lower level controllers use these commands as targets for (some of) their tasks. Lower level controllers can update states while higher level controllers use these states as inputs. Doing so one can implement a cascade of controllers, because control decisions come from higher levels and go to lower levels in a hierarchical way.

This cascading architecture is quite common in robotics, in fact it is found in mostly all roboctics application where a high level controller control joints but those joints are in fact an abstraction of a motor drive unit (an electronic device) that controls joint position/force/velocity by piloting a corresponding motor (or more).

In this section we do not consider specifically cascading controllers but more generally any kind of controller architecture involving interacting controllers as this makes absolutely no difference in RoboCoP. One can imagine a shared control architecture (where all controllers would send commands/receive states from all the others), or an hybrid architecture mixing previous styles, this is of no importance for what is explained after.

The only thing to understand with interacting controllers: they interact through joints. Indeed what a controller does is to update joint commands using specific algorithms. To do so a controller also sometimes need to have updated joint states to work with most up to date values. In RoboCoP this joint state update is not directly under the responsibility of the controller but on the elements it uses: most of time drivers but also why not algorithms. These joint states, possibly together with other sensors values, are used for instance to update robot bodies state (i.e. using forward kinematics). Then all these informations are used by the controller, either directly (i.e. jacobians computed for current joint state) or by its tasks.

Depending on the architecture chosen, controllers will have to read or write some joint commands and some joint states.

An interesting point to discuss right now is: how would we need interacting controllers ? One simple answer can be: my controller cannot manage such types of structure. For instance your robot is a mobilemanipulator composed of a wheeled mobile base and a serial arm mounted on it. The whole body robot controller could for instance control the arm joints but is not capable of controlling the mobile base wheels in the corretc way. Insteald it can control a virtual 3 DOFs joint representing mobile base motion relative to the world:

  • the low level controller controls the mobile base motions motion by reading commands on the virtual 3 DOFs joint and update this virtual joint state (2DOF position and velocity in reference frame) using odometry.
  • the high level controller controls the arm and the 3 DOF virtual joint, by generating commands for them. It also need to read the joint state to correctly perform its tasks involving mobile base motion (e.g. body tasks referenced in the world).

This example is drawn if the following figure:

cascading controllers

virtual joints is an important feature to nicely describe interacting controllers, because they allow to abstract sub parts of a complex robotic harware architecture that are controlled using specific controllers.

describing the world of each controllers

Let’s suppose we have interacting controllers, using virtual joints. One benefit of virtual joints is that they allow to abstract away part of the robot and thus simplify description for a higher level controller like in the previous mobile manipulator example (or more generally for any controller interacting with it). This simplification should appear in the world used by the higher level controller: only elements important for a controller should be desrcibed in its world. If we refer to mobile manipulator example (see previous figure):

  • The world for whole body controller contains description of the arm and description of the vitual 3 DOF joint for the mobile base
  • The world for the mobile base contains full description of the mobile base, so it contains description of the 3 DOFs virtual joint but also the real joints in the mobile platform hardware description.

So in the end each world should contain all data necessary for a given controller. That means we should be capable of describing variants of the same world.

To do that, your application has to declare mutliple worlds directly in the application configuration file. For the mobile manipulator example:

whole_body:
  models:
    - include: robocop-example-description/models/mobile_base.urdf.yaml
      namespace: mobile
    - path: robocop-example-description/models/kuka_lwr.urdf
      namespace: arm
      joint:
        parent: mobile_body

  processors:
    controller:
      type: robocop-tutorial-controller/processors/tutorial-controller
      options:
        joint_group: lwr_joints

    model:
      type: robocop-model-ktm/processors/model-ktm
      options:
        implementation: pinocchio
        input: state
        forward_kinematics: true
        forward_velocity: false

mobile_base:
  models:
    - include: robocop-tutorial-controller/models/mobile_base_complete.urdf.yaml
      namespace: mobile

  processors: 
    ...

Each world has a unique name, here whole_body and mobile_base the two worlds describing all elements for respectively the whole body controller and the mobile base specific controller. Those two worlds are completely independent, they can contain whatever is needed to implement each controller. Just rememebr that your application code needs in the end to copy data from one world to another in order to make the corresponding controller interacting. The best way is so to keep models as close as possible: the mobile_base_complete.urdf.yaml should be quite close to the description of mobile_base.urdf.yaml: bothe should contains the description of the 3 DOFS joint but mobile_base_complete.urdf.yaml also contains a full description of the mobile base hardware structure (corresponding to the right of previous figure).

In the end you have two separated worlds (with corresponding class names): whole_body and mobile_base whith their own structure and processors ready to be used. To use them simply do:

#include <robocop/controllers/wb_controller.h>
#include <robocop/controllers/mb_controller.h>
#include <robocop/model/pinocchio.h>
#include "robocop/whole_body.h"
#include "robocop/mobile_base.h"

int main() {
    using namespace phyq::literals;

    const phyq::Period time_step = 10_ms;

    auto whole_body = robocop::whole_body{};
    auto model = robocop::ModelKTM{whole_body, "model"};
    auto wb_controller =
        robocop::WBController{whole_body, model, time_step, "controller"};

    auto mobile_base = robocop::mobile_base{};
    auto model2 = robocop::ModelKTM{mobile_base, "model"};
    auto mb_controller =
        robocop::MBController{mobile_base, model, time_step, "controller"};

    //...
}

Where robocop::WBController and robocop::MBController are the two different controller for whole body control an dmobile base control respectively. Compared to the classical one world description now each independent world has its own name and corresponding classes: robocop::whole_body and robocop::mobile_base, generated by code generator from description.

Asynchronous execution

One you have multiple worlds you need to execute them in saparated threads and to synchronize them. Threading is not defined by RoboCoP itself so you can choose any thread model you prefer. In each thread you have to implement the controller periodic loop more or less the same way as you do usually with RoboCop, nothing change for a given controller.

So something that looks like:

...
int main() {
    using namespace phyq::literals;

    const phyq::Period time_step = 10_ms;

    auto whole_body = robocop::whole_body{};
    auto model = robocop::ModelKTM{whole_body, "model"};
    auto wb_controller =
        robocop::WBController{whole_body, model, time_step, "controller"};

    
    auto mobile_base = robocop::mobile_base{};
    auto model2 = robocop::ModelKTM{mobile_base, "model"};
    auto mb_controller =
        robocop::MBController{mobile_base, model, time_step, "controller"};

    //real time loop for whole body controller
    std::thread t_whole_body([&]{
        while(not end){
            //update joints/body states 
            //call wb_controller
            //update joint commands
        }
    });
    //real time loop for mobile base controller
    std::thread t_mobile_base([&]{
        while(not end){
            //update joints/body states 
            //call mb_controller
            //update joint commands
        }
    });
    
    t_whole_body.join();
    t_mobile_base.join();
}

At this point the two controllers are still independant, without any synchronziation. The only thing that remains to be done is to to read/write data correctly between worlds. In the example the virtual 3 DoFs joint representing mobile motion in a 2D world is available in both worlds. What we need to to is simply to copy its internal values from one world to another at adequate moments:

  • in mobile base thread, when updating joint state, the value of the virtual joint command is 1) copied from a buffer shared with whole body thread and 2) used to update the mobile base (body velocity) task target.
  • in mobile base thread, when updating joint commands, the virtual joint state is updated from computed odometry, then it is copies to a buffer shared with whole body thread.
  • in whole body thread, when updating joint state, the value of the virtual joint state is read from the buffer shared with mobile base thread in orer to update the local virtual joint state.
  • in whole body thread, when updating joint command, the value of the virtual joint command is written in the buffer shared with mobile base thread.

To achieve the synchronziation between threads/worlds, RoboCoP core library provides classes AsyncWorldReader and AsyncWorldWriter. They are used to store data in intermediate buffers used respectively to reading world data or to write world data. Each of these class must be specialized given your application needs to implement correct data copy. For instance copying virtual joint command written in whole body controller world into a buffer later read to update either same virtual joint command in mobile base world (reflecting exchanged information) or corresponding task target in mobile base world. Both case are identical but the later case leaves more flexibility because the chosen task that implements the virtual joint can be changed during mobile base world execution.