Compiletime (constexpr) Differential Equation System Description for use in Optimal Control

Question

Background

In the field of optimal control, it is part of everyday business to solve ordinary differential equations numerically. A differential equation system or dynamic system describes e.g. the physical behavior of an object over time. Every single equation can often be associated with a special state (position, velocity, acceleration, ...). Additionally, there are the "controls" which are the influenceable variables in the system.

In the development process, it is not always immediately clear how many and which states have to be used and also not exactly what controls and what states are. Therefore, this is often experimented with and the associated code is often changed.

Therefore I want to create a software library for me and my colleagues, which makes it easy to adjust the level of detail of the equations in a system. Faulty equations would often not lead to errors at runtime but only to strange results. Therefore one goal of the library is that the compiler should already perform as many checks as possible.

The library will later be used to map states and controls to the matrix and vector indices in a NLP-Solver. So it is important that they have consecutive indices in the background. However, this is one of the main things I really want to hide from the user.

It is also my constexpr learning project.

Example for Situation "as is"

The following code shows a part of a class which needs to be provided for an Optimal Control Solver. It shows the function to define the ODE System and also a function to provide the Derivative-Structure. The seond one means the user defines at which Entries the Jacobian Matrix of the ODE System has non zero entries.

My library is not ready yet to provide this structure. However, I still show it here because it shows how important it is to get the indices right.

It is only a small section from a large example, and unfortunately it is not compilable. It is only meant to shed some light on the background and does not belong to the code I would like to have a review for.

It shows a system which describes a moving car in a 2D plane.

The problem we are repeatedly facing is that we change the ODE system by adding or removing states. For example if we try what happens if we do not consider the acceleration to be a control directly but more to act as a set-point control which introduces one states more an changes control and state indices. In the current case one would need to update all the indices, and if one index is wrong you will see weird behaviour in the optimization process which is really hard to pin down.

Also important: The function evalSystemDynamic runs a very hot loop in the system with high demands on the time.

const std::size_t idx_xPos = 0;
const std::size_t idx_yPos = 1;
const std::size_t idx_orientation = 2;
const std::size_t idx_velocity = 3;
const std::size_t idx_steeringAngle = 4;

const std::size_t idx_ctrl_acceleration = 0;
const std::size_t idx_ctrl_steeringAngleAcceleration = 1;

const std::size_t idx_const_finalTime = 0;

const double radstand = 2.768;

void SingleTrackModelSimple::evalSystemDynamic(Span<double> newX, Span<const double> xAtT, Span<const double> uAtT, Span<const double> constants,
                                               Span<const double> /*perturbations*/, const o2c::DiscretizedTimePoint &/*timepointToEvaluate*/) const
{
    const auto orientation = xAtT[idx_orientation];
    const auto velocity = xAtT[idx_velocity];
    const auto steeringAngle = xAtT[idx_steeringAngle];

    const auto acceleration = uAtT[idx_ctrl_acceleration];
    const auto steeringAngleAcceleration = uAtT[idx_ctrl_steeringAngleAcceleration];

    newX[idx_xPos] = std::cos(orientation) * velocity;
    newX[idx_yPos] = std::sin(orientation) * velocity;
    newX[idx_orientation] = velocity * std::tan(steeringAngle) / radstand;
    newX[idx_velocity] = acceleration;
    newX[idx_steeringAngle] = steeringAngleAcceleration;

    const auto finalTime = constants[idx_const_finalTime];
    std::transform(newX.begin(), newX.end(), newX.begin(), [finalTime](const auto stateValue){ return stateValue * finalTime;});
}

void SingleTrackModelSimple::getSystemDynamicDerivativeStructure(o2c::TransWorhpDiffStructure &diffStruct, const o2c::StaticIndexHandler &idx) const
{
    diffStruct.setEntry(idx_xPos, idx.varIndex(idx_velocity));
    diffStruct.setEntry(idx_xPos, idx.varIndex(idx_orientation));
    diffStruct.setEntry(idx_xPos, idx.constantsIndex(idx_const_finalTime));

    diffStruct.setEntry(idx_yPos, idx.varIndex(idx_velocity));
    diffStruct.setEntry(idx_yPos, idx.varIndex(idx_orientation));
    diffStruct.setEntry(idx_yPos, idx.constantsIndex(idx_const_finalTime));

    diffStruct.setEntry(idx_orientation, idx.varIndex(idx_velocity));
    diffStruct.setEntry(idx_orientation, idx.varIndex(idx_steeringAngle));
    diffStruct.setEntry(idx_orientation, idx.constantsIndex(idx_const_finalTime));

    diffStruct.setEntry(idx_velocity, idx.ctrlIndex(idx_ctrl_acceleration));
    diffStruct.setEntry(idx_velocity, idx.constantsIndex(idx_const_finalTime));

    diffStruct.setEntry(idx_steeringAngle, idx.ctrlIndex(idx_ctrl_steeringAngleAcceleration));
    diffStruct.setEntry(idx_steeringAngle, idx.constantsIndex(idx_const_finalTime));
}

Code To Review

Prerequesites

My target platform is Linux and C++20, you can find compiling code under https://godbolt.org/z/3GY6q7

I use the gsl for gsl/span, which could also be replaced by std::span.

The Library

DynamicSystem.hpp

At the center of the software is the DynamicSystem class. It receives a list of states and a list of controls from the user. In addition, the type of ID can be identified via the states and controls.

During the design, I have tuples for the lists in my head, because the states and controls are single classes. For the IdType I have an enum in my head, because this guarantees the uniqueness. But this is ultimately up to the user.

#pragma once

#include "StateAccessor.hpp"

#include <algorithm>
#include <cassert>
#include <utility>
#include <tuple>

#include <gsl/span>

namespace cmplode
{

namespace detail
{

template<typename>
struct array_size;
template<typename T, size_t N>
struct array_size<std::array<T, N>>
{
  static size_t const size = N;
};

template<class States, class StateMapper, std::size_t I = 0, typename... Tp>
inline typename std::enable_if<I == sizeof...(Tp), void>::type
  ode([[maybe_unused]] const std::tuple<Tp...>& t, [[maybe_unused]] States& out, [[maybe_unused]] const StateMapper& in)
{}

template<class States, class StateMapper, std::size_t I = 0, typename... Tp>
  inline typename std::enable_if < I<sizeof...(Tp), void>::type
  ode(const std::tuple<Tp...>& t, States& out, const StateMapper& in)
{
  static_assert(I < std::tuple_size_v<States>);
  out[I] = std::get<I>(t).ode(in);
  ode<States, StateMapper, I + 1, Tp...>(t, out, in);
}

}// namespace detail

template<class IdType, class StateList, class ControlList>
class DynamicSystem
{
public:
  constexpr DynamicSystem(StateList states_, ControlList /*controlTypes_*/) :
    states(std::move(states_))
  {
  }

  constexpr auto getNumberOfStates() const
  {
    return std::tuple_size_v<StateList>;
  }

  constexpr auto getNumberOfControls() const
  {
    return std::tuple_size_v<ControlList>;
  }

  template<class States, class Controls>
  constexpr void ode(States& lhs, const States& stateValues, const Controls& controlValues) const
  {
    const StateAccessor<IdType, StateList, ControlList> stateMapper(stateValues, controlValues);
    detail::ode(states, lhs, stateMapper);
  }

private:
  StateList states;
};

}// namespace cmplode

StateAccessor.hpp

The StateAccessor also receives the states and controls as a list. It is intended to map from an IdType id to an internal index and hide the difference between state and control from the outside.

#pragma once

#include "OdeIndexMap.hpp"

#include <algorithm>
#include <cassert>
#include <tuple>
#include <gsl/span>

namespace cmplode
{

template<int... Indices>
struct indices
{
  using next = indices<Indices..., static_cast<int>(sizeof...(Indices))>;
};

template<int Size>
struct build_indices
{
  using type = typename build_indices<Size - 1>::type::next;
};

template<>
struct build_indices<0>
{
  using type = indices<>;
};

template<typename T>
using Bare = typename std::remove_cv<typename std::remove_reference<T>::type>::type;

template<typename Tuple>
constexpr
  typename build_indices<std::tuple_size<Bare<Tuple>>::value>::type
  make_indices()
{
  return {};
}

template<typename Tuple, int... Indices>
constexpr std::array<
  typename std::tuple_element<0, Bare<Tuple>>::type,
  std::tuple_size<Bare<Tuple>>::value>
  to_array(Tuple&& tuple, indices<Indices...>)
{
  using std::get;
  return { { get<Indices>(std::forward<Tuple>(tuple))... } };
}

template<typename Tuple>
constexpr auto to_array(Tuple&& tuple)
  -> decltype(to_array(std::declval<Tuple>(), make_indices<Tuple>()))
{
  return to_array(std::forward<Tuple>(tuple), make_indices<Tuple>());
}

template<typename... Ts, typename Function, size_t... Is>
constexpr auto transform_impl(std::tuple<Ts...> const& inputs, Function function, std::index_sequence<Is...>)
{
  return std::tuple<std::result_of_t<Function(Ts)>...>{ function(std::get<Is>(inputs))... };
}

template<typename... Ts, typename Function>
constexpr auto transform(std::tuple<Ts...> const& inputs, Function function)
{
  return transform_impl(inputs, function, std::make_index_sequence<sizeof...(Ts)>{});
}

template<class StateList>
constexpr auto create_id_array(const StateList& usedStates)
{
  return to_array(transform(usedStates, [](const auto state) { return state.id; }));
}

template<class IdType, class StateList, class ControlList>
class StateAccessor
{
public:
  template<class States, class Controls>
  constexpr StateAccessor(const States& states, const Controls& controls) :
    state_values{ gsl::span(states.data(), static_cast<gsl::span<const double>::size_type>(states.size())) },
    control_values{ gsl::span(controls.data(), static_cast<gsl::span<const double>::size_type>(controls.size())) }
  {
  }

  template<IdType id>
  constexpr double value_of() const
  {
    constexpr OdeIndexMap<IdType, std::tuple_size_v<StateList>> id_to_idx_states(create_id_array(StateList()));

    constexpr OdeIndexMap<IdType, std::tuple_size_v<ControlList>> id_to_idx_controls(create_id_array(ControlList()));

    if constexpr (id_to_idx_states.has_id(id)) {
      return state_values[id_to_idx_states.idx_of(id)];
    }

    if constexpr (id_to_idx_controls.has_id(id)) {
      return control_values[id_to_idx_controls.idx_of(id)];
    }

    assert(false && "Unkown Id");
    return 0.0;
  }

private:
  gsl::span<const double> state_values{};
  gsl::span<const double> control_values{};
};

}// namespace cmplode

OdeIndexMap.hpp

The OdeIndexMap is a constexpr map from IdType to a std::size_t index. The indices are sequential from 0 to the number of states.

#pragma once

#include <algorithm>

namespace cmplode
{

template<class IdList, class IdType>
constexpr auto compute_ode_index(const IdList& ids, IdType id)
{
  const auto posIt = std::find(std::begin(ids), std::end(ids), id);
  return std::distance(std::begin(ids), posIt);
}

template<class Idtype, std::size_t numberIds>
struct OdeIndexMap
{
  constexpr OdeIndexMap(std::array<Idtype, numberIds> usedIds_) :
    usedIds{ std::move(usedIds_) }
  {
  }

  std::array<Idtype, numberIds> usedIds;

  constexpr auto idx_of(Idtype wantedId) const
  {
    return compute_ode_index(usedIds, wantedId);
  }

  constexpr bool has_id(Idtype id) const
  {
    return std::find(std::begin(usedIds), std::end(usedIds), id) != std::end(usedIds);
  }
};

}// namespace cmplode

User Code

I created a small example for a system which describes a 1D movement with the states Position and Velocity and the control Acceleration.

States.hpp

States.hpp represents a library done by the user. The idea is that users create the possible states for their domain once and later put together systems from the possible states.

#pragma once

#include <cstdint>
#include <tuple>

namespace cmplode
{

enum class state_ids {
  position_x = 0,
  velocity = 1,
  acceleration = 2
};

class position_x
{
public:
  constexpr static state_ids id = state_ids::position_x;

  template<class States>
  constexpr static double ode(const States& in)
  {
    return in.template value_of<state_ids::velocity>();
  }
};

class velocity
{
public:
  constexpr static state_ids id = state_ids::velocity;

  template<class States>
  constexpr static double acc_from(const States& in)
  {
    return in.template value_of<state_ids::acceleration>();
  }

  template<class States>
  constexpr static double ode(const States& in)
  {
    return acc_from(in);
  }
};

class acceleration
{
public:
  constexpr static state_ids id = state_ids::acceleration;

  template<class States>
  constexpr static double ode(const States& /*in*/)
  {
    return 3.0;
  }
};

}// namespace cmplode

main.cpp

I created a main.cpp to get compiling code in the compiler explorer. However, it is not the actual usecase since that one is more complex and hard to add completely to compiler explorer (which I want to do, to get some insight).

#include "States.hpp"
#include "DynamicSystem.hpp"

#include <array>
#include <tuple>
#include <iostream>

using id = cmplode::state_ids;

int main()
{
  constexpr std::tuple<cmplode::position_x, cmplode::velocity> diffGl;
  constexpr std::tuple<cmplode::acceleration> controlsTypes;

  constexpr cmplode::DynamicSystem<id, decltype(diffGl), decltype(controlsTypes)> dynamic(diffGl, controlsTypes);

  static_assert(dynamic.getNumberOfStates() == 2);
  static_assert(dynamic.getNumberOfControls() == 1);

  std::array states{ 0.0, 1.0 };
  constexpr std::array controls{ 2.0 };

  std::array statesOut{ 0.0, 1.0 };

  for (int iteration = 0; iteration < 1000; ++iteration) {

    dynamic.ode(statesOut, states, controls);

    std::transform(statesOut.begin(), statesOut.end(), states.begin(), states.begin(), [](const auto& stateDiff, const auto state) {
      return state + 1e-03 * stateDiff;
    });
  }

  std::copy(std::begin(states), std::end(states), std::ostream_iterator<double>(std::cout, " "));
}

Questions

I would be very interested in receiving feedback on the use of constexpr features. Especially if I am making something too complicated somewhere or if some functions can be replaced by existing STL functionality. Most of the basic constexpr and template functionality I got with googling and you will probably find it on Stack Overflow.

I would also be interested if there is a way around using the "template" keyword in States.hpp in the ode features. I already tried to make the value_of function a free function or use the id as a function parameter and not as a template. The latter prevented the constexpr if in the StateAccessor and the free function didn't work in my attempt.

You can also see a wrapper function in velocity in States.hpp for the acceleration. This helps but still requires the template keyword by the user at some place.

Overall I am not satisfied with the IdType system. The need to explicitly specify it in the templates, and thus the use of decltype, but also the only indirect connection of the enum to the state classes bothers me.

Answer 1

Your code is too complicated

I know ODEs, I know what control systems are, but sadly I have to say that knowing this doesn't make your code any easier to understand. I would not want to use this code, because the distance between how you would write a differential equation on paper and the code you have to write here is just too great. Let's start with main():

int main()
{
  constexpr std::tuple<cmplode::position_x, cmplode::velocity> diffGl;
  constexpr std::tuple<cmplode::acceleration> controlsTypes;

  constexpr cmplode::DynamicSystem<id, decltype(diffGl), decltype(controlsTypes)> dynamic(diffGl, controlsTypes);

Ok, I see you have a tuple of position and velocity, and a tuple of acceleration. So since the last one is named controlsTypes, I assume this is what you control in your system, and the rest just evolves according to the control variables. You then create a "dynamic system" (I think you mean dynamical system) of these variables. But where is the relation between all of them?

I have to look in States.hpp to figure out that cmplode::position_x is a type that has a function ode() that reads the value of state_ids::velocity from in. Ok but position is not velocity, position is velocity multiplied by time. Where is the time evolution done? There is nothing in any of the .hpp files that do any calculations whatsoever. In fact, the only thing your constexpr template machinery does is shifting variables! The real calculation is done inside the loop in main():

  for (int iteration = 0; iteration < 1000; ++iteration) {
    dynamic.ode(statesOut, states, controls);

    std::transform(statesOut.begin(), statesOut.end(), states.begin(), states.begin(), [](const auto& stateDiff, const auto state) {
      return state + 1e-03 * stateDiff;
    });
  }

Compare that to the following where the exact same calculation is performed:

  std::array variables{0.0, 1.0, 2.0}; // position, velocity, acceleration

  for (int iteration = 0; iteration < 1000; ++iteration) {
    std::transform(variables.begin(), variables.end() - 1, variables.begin() + 1, variables.begin(), [](auto variable, auto derivative) {
      return variable + 1e-03 * derivative;
    });
  }

That's it! Now, there is some flexibility in your code, if you add more templated classes to States.hpp, but I don't really see how this extremely complicated way to add things there is safer and less error prone than just writing out this code in the most naive way:

double position = 0.0;
double velocity = 1.0;
const double acceleration = 2.0;
const double dt = 1e-3;

for (double t = 0; t < 1; t += dt) {
    position += velocity * dt;
    velocity += acceleration * dt;
}

std::cout << "Position at t = " << t << ": " << position << "\n";

The code does not prevent errors

Faulty equations would often not lead to errors at runtime but only to strange results. Therefore one goal of the library is that the compiler should already perform as many checks as possible.

I don't think your code prevents any errors. I can still accidentily make position_x::ode() return in.template value_of<state_ids::acceleration>(). A mistake like this is easy if someone copy&pastes one class to make another one, and forgets to update some part of the new class. This code will compile fine of course.

But there is another issue: I can create state_ids with duplicate entries, like so:

enum class state_ids {
  position_x = 0,
  velocity = 1,
  acceleration = 1
};

This will compile without errors or warnings, nor will there be any runtime errors, but it will cause strange results.

Use of constexpr

I would be very interested in receiving feedback on the use of constexpr features. Especially if I am making something too complicated somewhere or if some functions can be replaced by existing STL functionality.

Making functions and variables constexpr is usually a good thing. But don't go out of your way to make code much more complicated than necessary just to make it constexpr, unless you have a very good reason to do so. A constexpr function is not guaranteed to be run at compile-time; the compiler can decide to evaluate them at run-time. Conversely, a compiler could take a non-constexpr function, deduce that it can calculate everything it does at compile-time, and just have it return a pre-compiled answer.

Avoiding the template keyword

I would also be interested if there is a way around using the "template" keyword in States.hpp in the ode features.

The reason you need this is because position_x::ode() is a template, not a real function. So at the time the compiler tries to parse it, it does not know the type of in, it can be anything after all, so it also doesn't know what type in.value_of is. In particular, in order to correctly parse the token <, it has to know if value_of is a template or not. If it is not, it would assume < is the lesser-than operator. At instatiation time it will use the already parsed tokens, it will not reread the actual lines of code, and this will cause an error, unless you manually add the template keyword.

I do not see a way around this here, a free-standing function would also need to be a template and thus will have exactly the same problem. The only other solution is to make id a regular parameter of StateAccessor::value_of(), but of course that would prevent you from using constexpr if-statements.

Alternatives to the IdType system

Overall I am not satisfied with the IdType system. The need to explicitly specify it in the templates, and thus the use of decltype, but also the only indirect connection of the enum to the state classes bothers me.

Indeed, it would be better to avoid this, and intead make it possible to just refer to variables directly by name? Again, I would go back to basics, and try to avoid involving templates unless it is really necessary. You could create a struct with the variables of the system, and then just refer to the variable names when updating the state:

struct {
    double position{0.0};
    double velocity{1.0};
    double acceleration{2.0};

    void update(double dt) {
        position += velocity * dt;
        velocity += acceleration * dt;
    }
} system;

Or make it more abstract and just define the ODE as an array, with each element always being the derivative of the previous element:

struct {
    std::array<double, 3> variables;

    void update(double dt) {
        for (size_t i = 0; i < variables.size() - 1; ++i)
            variables[i] += variables[i + 1] * dt;
    }
} system;