Commit 44093d43 authored by Gauthier Quesnel's avatar Gauthier Quesnel
Browse files

core: introduce better observation system

The new observation system provides better outputs. It relies on all QSS
integrator values (X, u, mu and pu). Observers callback can compute
intermediate values instead of interpolation.
parent 69fabcad
Pipeline #29821 passed with stage
in 1 minute and 43 seconds
......@@ -231,6 +231,7 @@ struct plot_output
void operator()(const irt::observer& obs,
const irt::dynamics_type /*type*/,
const irt::time tl,
const irt::time t,
const irt::observer::status s);
......@@ -239,7 +240,7 @@ struct plot_output
std::vector<float> ys;
small_string<24u> name;
double tl = 0.0;
double time_step = 0.1;
double time_step = 0.01;
};
struct file_output
......@@ -252,6 +253,7 @@ struct file_output
void operator()(const irt::observer& obs,
const irt::dynamics_type /*type*/,
const irt::time tl,
const irt::time t,
const irt::observer::status s);
......
......@@ -15,8 +15,8 @@
#include "node-editor.hpp"
#include <chrono>
#include <fstream>
#include <filesystem>
#include <fstream>
#include <string>
#include <fmt/format.h>
......@@ -27,33 +27,83 @@
namespace irt {
static void
push_data(std::vector<float>& xs, std::vector<float>& ys,
const double x, const double y) noexcept
{
if (xs.size() < xs.capacity()) {
xs.emplace_back(static_cast<float>(x));
ys.emplace_back(static_cast<float>(y));
}
}
static void
pop_data(std::vector<float>& xs, std::vector<float>& ys) noexcept
{
ys.pop_back();
xs.pop_back();
}
void
plot_output::operator()(const irt::observer& obs,
const irt::dynamics_type /*type*/,
const irt::dynamics_type type,
const irt::time tl,
const irt::time t,
const irt::observer::status s)
{
switch (s) {
case irt::observer::status::initialize:
if (s == irt::observer::status::initialize) {
xs.clear();
ys.clear();
xs.reserve(4096u);
ys.reserve(4096u);
tl = t;
break;
case irt::observer::status::run: {
const float value = static_cast<float>(obs.msg[0]);
xs.reserve(4096u * 4096u);
ys.reserve(4096u * 4096u);
return;
}
while (!xs.empty() && xs.back() == tl)
pop_data(xs, ys);
switch (type) {
case irt::dynamics_type::qss1_integrator: {
for (auto to_fill = tl; to_fill < t; to_fill += time_step) {
const auto e = to_fill - tl;
const auto value = obs.msg[0] + obs.msg[1] * e;
push_data(xs, ys, to_fill, value);
}
const auto e = t - tl;
const auto value = obs.msg[0] + obs.msg[1] * e;
push_data(xs, ys, t, value);
} break;
case irt::dynamics_type::qss2_integrator: {
for (auto to_fill = tl; to_fill < t; to_fill += time_step) {
ys.emplace_back(value);
xs.emplace_back(static_cast<float>(t));
const auto e = to_fill - tl;
const auto value = obs.msg[0] + obs.msg[1] * e +
(obs.msg[2] * e * e / 2.0);
push_data(xs, ys, to_fill, value);
}
const auto e = t - tl;
const auto value =
obs.msg[0] + obs.msg[1] * e + (obs.msg[2] * e * e / 2.0);
push_data(xs, ys, t, value);
} break;
tl = t;
case irt::dynamics_type::qss3_integrator: {
for (auto to_fill = tl; to_fill < t; to_fill += time_step) {
const auto e = to_fill - tl;
const auto value = obs.msg[0] + obs.msg[1] * e +
(obs.msg[2] * e * e / 2.0) +
(obs.msg[3] * e * e * e / 3.0);
push_data(xs, ys, to_fill, value);
}
const auto e = t - tl;
const auto value = obs.msg[0] + obs.msg[1] * e +
(obs.msg[2] * e * e / 2.0) +
(obs.msg[3] * e * e * e / 3.0);
push_data(xs, ys, t, value);
} break;
case irt::observer::status::finalize:
ys.emplace_back(static_cast<float>(obs.msg[0]));
xs.emplace_back(static_cast<float>(t));
default:
push_data(xs, ys, t, obs.msg[0]);
break;
}
}
......@@ -61,6 +111,7 @@ plot_output::operator()(const irt::observer& obs,
void
file_output::operator()(const irt::observer& obs,
const irt::dynamics_type /*type*/,
const irt::time tl,
const irt::time t,
const irt::observer::status s)
{
......@@ -68,8 +119,6 @@ file_output::operator()(const irt::observer& obs,
switch (s) {
case irt::observer::status::initialize:
tl = t;
if (ed && !ed->observation_directory.empty())
file = ed->observation_directory;
......@@ -83,7 +132,6 @@ file_output::operator()(const irt::observer& obs,
case irt::observer::status::run:
if (ofs.is_open())
fmt::print(ofs, "{},{}\n", t, obs.msg[0]);
tl = t;
break;
case irt::observer::status::finalize:
if (ofs.is_open()) {
......
......@@ -35,6 +35,7 @@ struct file_output
void operator()(const irt::observer& obs,
const irt::dynamics_type /*type*/,
const irt::time tl,
const irt::time t,
const irt::observer::status s) noexcept
{
......
......@@ -35,6 +35,7 @@ struct file_output
void operator()(const irt::observer& obs,
const irt::dynamics_type /*type*/,
const irt::time tl,
const irt::time t,
const irt::observer::status s)
{
......
......@@ -35,6 +35,7 @@ struct file_output
void operator()(const irt::observer& obs,
const irt::dynamics_type /*type*/,
const irt::time tl,
const irt::time t,
const irt::observer::status s)
{
......
......@@ -2478,7 +2478,7 @@ struct observer
using update_fn =
function_ref<void(const observer&, const dynamics_type,
const time, const observer::status)>;
const time, const time, const observer::status)>;
observer(const char* name_, update_fn cb_) noexcept
: cb(cb_)
......@@ -2486,7 +2486,6 @@ struct observer
{}
update_fn cb;
double tl = 0.0;
small_string<8> name;
model_id model = static_cast<model_id>(0);
message msg;
......@@ -6478,8 +6477,7 @@ public:
while (observers.next(obs)) {
if (auto* mdl = models.try_to_get(obs->model); mdl) {
obs->msg.reset();
obs->cb(*obs, mdl->type, t, observer::status::initialize);
obs->tl = t;
obs->cb(*obs, mdl->type, mdl->tl, t, observer::status::initialize);
}
}
......@@ -6559,6 +6557,17 @@ public:
time t,
flat_list<output_port_id>& emitting_output_ports) noexcept
{
if constexpr (is_detected_v<observation_function_t, Dynamics>) {
if (mdl.obs_id != static_cast<observer_id>(0)) {
if (auto* obs = observers.try_to_get(mdl.obs_id); obs) {
obs->msg = dyn.observation(t - mdl.tl);
obs->cb(*obs, mdl.type, mdl.tl, t, observer::status::run);
} else {
mdl.obs_id = static_cast<observer_id>(0);
}
}
}
if (mdl.tn == mdl.handle->tn) {
if constexpr (is_detected_v<lambda_function_t, Dynamics>) {
if constexpr (is_detected_v<has_output_port_t, Dynamics>) {
......@@ -6584,18 +6593,6 @@ public:
if (auto* port = input_ports.try_to_get(dyn.x[i]); port)
port->messages.clear();
if constexpr (is_detected_v<observation_function_t, Dynamics>) {
if (mdl.obs_id != static_cast<observer_id>(0)) {
if (auto* obs = observers.try_to_get(mdl.obs_id); obs) {
obs->msg = dyn.observation(t - mdl.tl);
obs->cb(*obs, mdl.type, t, observer::status::run);
obs->tl = t;
} else {
mdl.obs_id = static_cast<observer_id>(0);
}
}
}
irt_assert(mdl.tn >= t);
mdl.tl = t;
......@@ -6628,8 +6625,7 @@ public:
{
if constexpr (is_detected_v<observation_function_t, Dynamics>) {
obs.msg = dyn.observation(t - mdl.tl);
obs.cb(obs, mdl.type, t, observer::status::finalize);
obs.tl = t;
obs.cb(obs, mdl.type, mdl.tl, t, observer::status::finalize);
}
}
......
......@@ -25,6 +25,7 @@ struct file_output
void operator()(const irt::observer& obs,
const irt::dynamics_type /*type*/,
const irt::time tl,
const irt::time t,
const irt::observer::status s)
{
......
......@@ -34,6 +34,7 @@ struct file_output
void operator()(const irt::observer& obs,
const irt::dynamics_type /*type*/,
const irt::time tl,
const irt::time t,
const irt::observer::status s) noexcept
{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment