Overview

Tools to help writing simple robocop applications. Includes state machine management and simple GUI genetaion, with mixed code customization and configuration files based dynamic configuration.

Using app_utils for application configuration

app_utils provides a way to easily configure simple robocop applications using YAML configuration files. Its primary intent is to simplify life of robocop application developpers, notably by trying to avoid at maximum code compilation, replacing it with configuration files modifications.

app_utils main features are:

• configuration files for application dynamic description without recompiling (as far as possible). Configuration files are used to configure all other aspects.
• a state machine for controlling how the configuration of the controller (tasks and constraints used) change along time.
• a GUI that is automatically generated when the application starts. This GUI basically provides a way to show state machine current state, to force state transition and exit application. With more advanced applications the GUI is also capable of showing and potentially controlling variables.
• concurrency for applications that require user code to run concurrentyy with the controller.

State machine : the basics

State machines are commonly used to dynamically change configuration of a controller typically by (de)activating tasks and constraints or tuning some of their parameters.

Example simple_app_example shows how to implement a very simple state machine in code (see file simple_app_example.cpp) and how to declare state machine transitions in configuration file simple_app_states.yaml:

states:
  init:
    initial: true
    next:
      second: first_pass
      third: otherwise
  second:
    next:
      third: second_elapsed
  third:
    next:
      : exit_cycle

states map contains the description of possible states : init, second, third. next field of states just give the possible transitions from one state to another, described as a YAML map. If many transitions are defined then the sequence give the priority of transition: from higher to lower priority. The starting state is marked as initial.

A next state can be either the name of a state or empty (see next field of state third), in this later case it means that the state can automatically goes to a default defined fallback state. The map values are condition functions (returning true if transition can be fired).

User code defines condition functions and actions performed when a state is entered, left and when it executes. For instance:

define_state("init",
    [this] {rel_time = 0_s;},
    [this] {...},
    [this](bool) {rel_time += time_step;return true;});
...
define_condition("first_pass", [this](bool) {return rel_time > 2 and time < 5;});

Please note that a single app can define a large amount of states and just a subset can be used in state machine.

Hierarchical state machine

It is also possible to create hierarchies of states machines : a state can itself launch an entire sub state machine. This is shown in parallel_app_example code and in configuration file parallel_app_states.yaml:

states:
  ...
  third:
    sub_states: sub_sm_1
    next:
      : exit_cycle
 
state_machines:
  sub_sm_1:
    state1:
      initial: true
      next:
        state2: go_to_state2
    state2:
      next:
        state3: go_to_state3
    state3:
      next:
        state1: go_to_state1
  ...

The state third has substates defined bu the state machine sub_sm_1. When execution enters in third state then sub_sm_1 is reset and then runs. When execution exit from third state sub_sm_1 is stopped. Code bound to transitions and states of these named state machines is described this way:

define_state_machine_state(
    "sub_sm_1", "state2",
    [this] { ... },
    [this] { ... },
    [this](bool) {...});
...
define_state_machine_condition(
    "sub_sm_1", "go_to_state2", [this](bool) { return sub_rel_time > 0.5;});

As you can notice this is mostly identical to previous way of doing, only difference is that you have to give the name of the corresponding state machine.

There is no restriction on the number of layers in the hierarchy.

Parallel execution

Using similar logic you can execute in parallel many state machines. This is also demonstrated in parallel_app_example:

states:
  ...
  second:
    sub_states: [sub_sm_1, sub_sm_2] #parallelism !
    next:
      third: second_elapsed
  ...

Here sub_states field of state second targets two state machines defined the same way as previously : they will be executed simultenaously while state second is active.

Please note that there is no true parallelism because execution is not threaded. Also please take care to avoid executing the same state machine more than once. But you can execute the same state machine in different non concurrent states.

State machine : advanced description

app_utils library provide extension mechanisms with the ControlApp class. Extension is beyond this quick overview, so the following explanations are based on the package robocop-qp-app-utils that provides an extension library qp_app_utils. This library provides an extension of ControlApp that automate the use of KinematicTreeController defined in robocop-qp-controller package. Following examples and configuration files can all be found in robocop-qp-app-utils.

Example tasks_based_app_example shows how to get a much more configurable application by allowing definitions of states directly inside the configuration file and how to connect these descriptions to real code. See file tasks_based_app_description.h to get the definition of procedures used into configuration file tasks_based_app_states.yaml:

The state machine looks like:

states:
   go_to_init:
    initial: true
    behavior: [joints_position_init, joints_kinematics_quick]
    next:
      stay: joints_position_reached
 
  go_to:
    behavior: [left_effector_goto, right_effector_goto, joints_kinematics_slow]
    next:
      stay: [left_end_effector_target_reached, right_end_effector_target_reached, stay_state_active] #it is and AND
 
  stay:
    behavior: [joints_position_stay, joints_kinematics_quick]
    next:
      sinus: [stay_state_not_active, sinusoidal_motion_ok, reset_total_duration]
      follow_user: [follow_state_active]
      go_to: stay_state_not_active
  ...
  sinus:
    cyclic: sinusoidal_motion_need_update
    behavior: [left_effector_stay, right_effector_stay, joints_kinematics_quick]
    next:
      stay: [sinusoidal_motion_ko, time_spent_greater_10_s]
 
fallback: #mandatory used to put the robot in safe state
  behavior: [joints_position_stay, joints_kinematics_quick]

Main differences with previous description are:

• behavior implemented by states is now explicit in configuration file. The behavior corresponds to the set of tasks and constraints used. These elements are defined in the configuration file.
• conditions are also defined directly in the configuration file.

All of this is explained in next sections.

Controller functions and configurations

Now we can specify in configuration file:

• the behavior field of states, that contains a unique or sequence of controller_configurations.
• the code used :
  • for cyclic activation of states.
  • for enter and exit actions to perform respectively just before behavior enabling and after behavior disabling.
  • for conditions bound to transitions. Each of these elements describe a unique or sequence of controller_functions.

cyclic, enter, exit and conditions are now refering to controller_functions defined in the configuration file:

controller_functions:
  stay_state_not_active:
    call: test_string_variable_differ
    set:
      var_to_test:
        variable: use_stay
      against:
        value: "stay"
 
  ...

Each controller_function (e.g. stay_state_not_active) simply calls (see field call) a procedure declared by the application (here test_string_variable_differ) and sets (see field set) some of its arguments either directly with a fixed value (e.g. parameter against is set to the value "stay") or with the value of a variable (e.g. parameter var_to_test is set to the value of the variable use_stay). Please note that every function returns a boolean, that should always be set to true if the function is not a condition or cyclic.

Similarly controller_configuration are defined in configuration files this way:

controller_configuration:
    ...
    joints_position_stay:
        task: "all_joints_position_control"
        type: joint_group #possible values: joint_group, body, generic
        call: set_joints_position_target
        set:
            joints_target_position:
                variable: current_joints_position
        priority: 0
    ...

The call and set fields have same meaning as for conditions. But the action also declares the task or constraint it uses and its type. An action consists in enabling the given task in controller when it gets used, giving it adequate additional configuration (e.g. priority of the task) and initializing its parameters by calling the procedure (here setting the value of the joint target). When the action is no more used (when state change) it automatically disables the task and reconfigures the controller as it was prior to enabling the task/constraint. This means that the controller has to define the corresponding task (see tasks_based_app_description.h):

auto& world = controller_.world();
controller.set_auto_enable(false);
...
auto& jg = world.joint_group("arms");
 
arms_joint_position_task = &controller_.add_task<robocop::JointPositionTask>(
                        "all_joints_position_control", jg);
jp_interpolator = &arms_joint_position_task->target()
    .interpolator().set<robocop::QuinticConstrainedInterpolator>(
        robocop::JointVelocity::constant(jg.joints().size(), 0.5),
        robocop::JointAcceleration::constant(jg.joints().size(),1.0),
        controller_.time_step());
 
jp_feedback = &arms_joint_position_task->feedback_loop()
                    .set_algorithm<robocop::ProportionalFeedback>();
jp_feedback->gain().resize(jg.dofs());
jp_feedback->gain().set_zero(); // value set in states

This is a classical code in robocop, nothing special here.

Conditions composition

With advanced applications, it is now possible to compose conditions activating transitions using simple and/or patterns together with controller functions. This is shown in concurrent_app_states.yaml:

states:
  ...
  stay:
    ...
    next:
      go_to: #it is an OR
        either: left_target_changed
        or: right_target_changed

Transition from state stay to state go_to is described using a composition of simpler conditions. In the example the transition to go_to is performed when either the left or right (or both) target has changed. This is achieved using a YAML map to express the or composition, each map entry being an alternative condition. Similarly you can use a YAML sequence to specify an and composition like in tasks_based_app_states.yaml:

states: #finally describe states that use conditions and actions
  ...
  go_to:
    behavior: [left_effector_goto, right_effector_goto, joints_kinematics_slow]
    next:
      stay: [left_end_effector_target_reached, right_end_effector_target_reached, stay_state_active] #it is and AND
  ...

The transition from go_to to stay is activated when left and right targets are reached, and also if stay_state_active returns true.

There is no restriction on the composition of conditions: you can mix and/or compositions by simply using YAML sequence or map in a tree like structure, with leafs being the defined conditions. Size of containers is also not restricted and elements of the containers do not need to be homogeneous (e.g. a map can contains a sequence, another map and a scalar).

Procedures

All of this relies on the definition of a set of procedures, that are also part of the configuration files. For instance:

procedures: #these are the real functions -> for wchich an implementation is required
    ...
    test_boolean_variable_true:
        parameters:
            var_to_test:
                type: bool
    ...
 
    test_string_variable_differ:
        parameters:
            var_to_test:
                type: string
            against:
                type: string

This is purely declarative, and just used to declare the parameters used by the procedure. User code (see tasks_based_app_description.h) has to provide an implementation for each declared procedure, for instance:

define_procedure("test_string_variable_differ", [this] {
           auto test_var = this->get_parameter<std::string>(
               "test_string_variable_differ", "var_to_test");
           auto against = this->get_parameter<std::string>(
               "test_string_variable_differ", "against");
           return test_var != against;
       });

The get_parameter function is accessible to this (application object), it is useed to get the value of the procedure declared parameters. For any parameter only predefined types can be used:

• primitive types: int, double, string, bool
• robocop types: JointPosition, SpatialPosition, JointVelocity, Duration, Force, etc.

app_utils in fact provides a vast amount of predefined procedures and their implementation, for all basic types of robocop, which should most of time help the user avoid writing code and declaring such trivial functions as test_boolean_variable_true and test_string_variable_differ. In controller_functions section of configuration files found in robocop-qp-app-utils you can find examples of these predefined procedures (e.g. pose_!=, pose_=, str_==, bool_true, etc.).

Each controller specific application like QPControlApp also automatically defines procedures for tasks declared in the application, with following pattern:

• <name fo task>_target_reached for task that conceptually have a termination.
• set_<name fo task>_target that allow user to set task targets. To understand what <name fo task> pattern refers to, please read next section.

Controller description

It is also possible to completely automate controller configuration without having to write a single line of code for that. That is demonstrated in example fully_configurable_qp_app_example.cpp. This is currently only possible with robocop kinematic-tree QP controller (using the class QPControlApp) but same kind of feature can be achieved for any kind of robocop controller if the corresponding extension class is provided.

Such feature allows to automatically configure a controller, for instance see file fully_configurable_qp_app_states.yaml:

controller:
    kinematic_constraint_slow:
      type: JointKinematicConstraint
      joint_group: all #special value
      parameters:
        min_position: limits #special value
        max_position: limits #special value
        max_velocity: 0.5
        max_acceleration: 5
    ...
    all_joints_position_control:
      type: JointPositionTask
      joint_group: arms
      interpolator:
        type: QuinticConstrainedInterpolator
        max_velocity: 0.5
        max_acceleration: 1.0
      feedback:
        type: ProportionalFeedback
        gain: 10.0
    ...

To be short, any simple configuration of the controller can be fully described this way and so is dynamic (does not require to recompile). More sophisticated configuration are still possible in code (as shown is previously example) letting maximum flexbility to the user.

Variables

One really important feature of configuration file based description is variables. A variable is a typed register declared from configuration file but accessible by code.

Here are the variables declared in file fully_configurable_qp_app_states.yaml:

variables:
    left_target_position:
        type: SpatialPosition
        unit: #only usefull for printing and control ecause no value specified
            linear: m
            angular: deg
        gui:
            print: true
            control: true
            limits:
                lower:
                    linear: [-1.5, -1, 0.3]
                    angular: [-90, -90, 0]
                upper:
                    linear: [1.5, 1, 1.8]
                    angular: [90, 90, 180]
    ... #other variables
    left_current_position:
        type: SpatialPosition
        gui:
           print: true
    ... #other variables

A variable has:

• a name (here left_target_position or left_current_position) that is unique for the application.
• a type (here SpatialPosition) that is mandatory.
• other informations (including an initial value) are optional.

The basic role of a variable is:

• to hold a dynamic value for calling procedures.
• to be monitored and set by user code, whenever necessary. For instance in fully_configurable_qp_app_example.cpp the function define_cycle_update is used to set the value of left_current_position at each cycle of the controller:

define_cycle_update([this] {
    auto& model = this->controller().model();
    auto& left_pose = model.get_body_position("left_link_7");
    //updating delcared variable from world state
    this->set_variable("left_current_position", left_pose);
    ...
});

Another important feature of variables is to be possibly known in application GUI. You can control if its value is printed (using print: true) and if you want to be capable of modifying its value "by hand", through a GUI control (slider or checkbox for instance depending of the nature of the variable), by setting its field control to true. Information about the variable in GUI will be set according to the unit specified for the variable.

Concurrency

The last important feature is to inform the application that the variable is shared with concurrent user code (see concurrent_app_states.yaml):

variables:
   left_target_position:
        type: SpatialPosition
        shared: true
        gui:
            print: true
        value:
            frame: world
 
    ... #other variables

In this example, using shared: true will protect the variable from accesses of a concurrent thread (i.e. whenever this thread uses get_variable and set_variable functions). For instance have a look in concurrent_app_example.cpp file:

define_user_code([this] {
    // using variables to exchange data with controller state machine
    pid::Period p{10ms};
    auto wait_stay_state = [this, &p] {
        p.reset();
        while (this->state() != "stay" and not this->end()) {
            p.sleep();
        }
        return not this->end();
    };
    ;
    if (not wait_stay_state()) {
        return;
    }
    // now in stay state
    int counter{0};
    auto left_tgt =
        get_variable<SpatialPosition>("left_current_position");
    auto right_tgt =
        get_variable<SpatialPosition>("right_current_position");
 
    SpatialVelocity motion{phyq::zero, left_tgt.frame()};
    motion.linear() << 0.1_mps, 0.1_mps, 0.1_mps;
    phyq::Duration<> motion_duration{0.5s};
 
    while (counter++ < 5 and not this->end()) {
        // move a bit;
        left_tgt += motion * motion_duration;
        right_tgt += motion * motion_duration;
        fmt::print(
            "NEW TARGETS -> left: {:f:r{euler}} right: {:f:r{euler}}\n",
            left_tgt, right_tgt);
        set_variable("left_target_position", left_tgt);
        set_variable("right_target_position", right_tgt);
        std::this_thread::sleep_for(1s); // wait for state change
        if (not wait_stay_state()) {
            return;
        }
    }
});

define_user_code is used to let the user define its application specific code that will be synchronized with controller state machine.

When state machine first time reaches the "stay" state we get the value of left_current_position and right_current_position using get_variable. We use it as initial position that will be incremented to define targets (left_target_position and right_target_position set using set_variable). When a variable is declared to be shared, then the call to get_variable and set_variable protects the access to the variable memory.

Shared variables are so one way to synchronize user code and controller state machine.

The only other is to use state machine control operations, such as end()(state machine execution is finished), state() (get the current state of the state machine), cycles() (number of execution cycles) and force_exit_state() (force exit current state to the selected possble next state).

Debugging

By default app_utils only shows the log of state transitions to the user which is not sufficient if you want to understand what is going wrong during execution.

app_utils provides a way to control debugging of the application execution. This basically consist in using debug: true in various descriptive elements of the configuration file. You can have a look at concurrent_app_states.yaml configuration file, that demonstrates where you can put debug info.

Here is a sum up:

• at root level debug help understanding why transitions from one state to another are performed (giving result of evaluation of conditions and cyclic when it returns false)
• in states of the (sub)state machine(s), this prints the complete configuration of the controller (active tasks and constraints) when state is enabled.
• in elements of controller_functions and controller_configuration it prints the call (functions and arguments values) of enter, exit functions as well as configuration of tasks.

The license that applies to the whole package content is CeCILL-C. Please look at the license.txt file at the root of this repository for more details.

About authors

robocop-app-utils has been developed by following authors:

• Robin Passama (CNRS/LIRMM)

Please contact Robin Passama (robin.nosp@m..pas.nosp@m.sama@.nosp@m.lirm.nosp@m.m.fr) - CNRS/LIRMM for more information or questions.