Minimal Controller#

The demo_minimal_controller application provides a minimalistic example of the code in the controller namespace for Model Predictive Control of a simple simulator. The Constraint Free Simulator (CFS) is simply a 'point' moving through space, with 6 degrees of freedom, constrained by minimum and maximum values for each degree of freedom.

The example shows:

  1. how to create a Controler object for the type of simulator to be controlled,
  2. the basic configuration options,
  3. how to call the update methods of the controller, and
  4. the way in which the state of the simulator needs to be updated for the next control cycle.

The example is documented in comments in the code.

  1/**
  2 * Copyright Frank Drop and Mario Olivari ("Predictive cueing").
  3 * Please read carefully the terms and conditions (root/LICENCE.md) and
  4 * any accompanying documentation before you download and/or use
  5 * the Model Predictive Motion Cueing Algorithm (MPMCA) software.
  6 */
  7#include "mpmca/control/controller.hpp"
  8#include "mpmca/control/simulator.hpp"
  9#include "mpmca/control/state.hpp"
 10#include "mpmca/linear_algebra.hpp"
 11#include "mpmca/utilities/logger.hpp"
 12#include "mpmca_constraint_free_simulator/mpmca_constraint_free_simulator.hpp"
 13
 14int main(int argc, char** argv)
 15{
 16    using namespace mpmca;
 17    using CFS = control::MpmcaConstraintFreeSimulator;
 18
 19    utilities::Logger logger("ControllerExample", "Main");
 20
 21    Vector<12> initial_state = Vector<12>::Zero();
 22    Vector<6> initial_input = Vector<6>::Zero();
 23
 24    unsigned horizon_length = 50;
 25    unsigned horizon_step = 1;
 26    double horizon_dt = 0.01;
 27
 28    control::Controller<CFS> controller(
 29        horizon_length, horizon_step, horizon_dt, initial_state, initial_input);
 30
 31    control::Simulator<CFS> simulator;
 32
 33    // Although the controller will run fine without explicitly setting the
 34    // controller weights, it is recommended to do so.
 35    CFS::OutputVector output_error_weight;
 36    CFS::StateVector state_error_weight;
 37    CFS::InputVector input_error_weight;
 38    output_error_weight << 1, 1, 1, 10, 10, 10, 0, 0, 0;
 39    state_error_weight << 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
 40    input_error_weight << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
 41
 42    controller.SetOutputErrorWeight(output_error_weight);
 43    controller.SetStateErrorWeight(state_error_weight);
 44    controller.SetInputErrorWeight(input_error_weight);
 45
 46    Vector<12> current_state = initial_state;
 47    Vector<12> next_state;
 48
 49    // The control namespace provides a lot of "viewers". These objects allow
 50    // you to access the elements of InputVectors, StateVectors, InfoVectors,
 51    // and ConstraintVector more easily by name. The ViewAt template index
 52    // refers to the index of all Components of a simulator. That is, certain
 53    // simulators consist of multiple Components stacked 'on top of each other'.
 54    // For example, the MpmcaXyHexapodYaw simulator consists of a XyTable, a
 55    // Hexapod, and a YawTable.
 56    auto next_state_view = control::MakeStateView<CFS>(next_state).ViewAt<0>();
 57
 58    double sine_frequency = 5;  // Hertz
 59
 60    CFS::OutputVector reference_output;
 61
 62    for (double time = 0; time < 1.0; time += horizon_dt) {
 63        // Generate a sinewave and use it for the reference output
 64        for (size_t kk = 0; kk < controller.GetMpcHorizonLength(); ++kk) {
 65            double horizon_time = time + horizon_dt * kk;
 66            double sine_value =
 67                horizon_time *
 68                std::sin(2 * 3.141 * sine_frequency * horizon_time);
 69
 70            reference_output << 0, 0, 9.81 + sine_value, 0, 0, 0, 0, 0, 0;
 71            controller.SetReferenceOutput(kk, reference_output);
 72        }
 73        // The controller needs to be Prepared before calculating the next
 74        // control input in the call to Feedback. These calls are independent
 75        // calls, because it is possible to parallelize the Preparation call.
 76        controller.Preparation();
 77        // The return value of Feedback is the next input to be given to the
 78        // system. That is, the controller calculates the optimal inputs for
 79        // the entire control horizon, but only the first is used for
 80        // controlling the system (simulator).
 81        auto const first_input = controller.Feedback(current_state);
 82
 83        // The simulator object can be used to integrate the state using the
 84        // calculated input.
 85        next_state =
 86            simulator.Integrate(current_state, first_input, horizon_dt);
 87
 88        // Note that you can also get the next state from the
 89        // controller directly, by getting the state from the StatePlan at
 90        // position 1.
 91        auto const next_state_controller = controller.GetStatePlan(1);
 92        auto next_state_controller_view =
 93            control::MakeStateView<CFS>(next_state_controller).ViewAt<0>();
 94
 95        logger.Info("At time " + std::to_string(time) +
 96                    " the z-position of the simulator is: " +
 97                    std::to_string(next_state_view.GetZ()) +
 98                    ", the z-position calculated by the controller is: " +
 99                    std::to_string(next_state_controller_view.GetZ()));
100
101        // This is the end of the current sample, thus we update current_state
102        // to next_state.
103        current_state = next_state;
104    }
105
106    return 0;
107}