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 ...@@ -231,6 +231,7 @@ struct plot_output
void operator()(const irt::observer& obs, void 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::time t,
const irt::observer::status s); const irt::observer::status s);
...@@ -239,7 +240,7 @@ struct plot_output ...@@ -239,7 +240,7 @@ struct plot_output
std::vector<float> ys; std::vector<float> ys;
small_string<24u> name; small_string<24u> name;
double tl = 0.0; double tl = 0.0;
double time_step = 0.1; double time_step = 0.01;
}; };
struct file_output struct file_output
...@@ -252,6 +253,7 @@ struct file_output ...@@ -252,6 +253,7 @@ struct file_output
void operator()(const irt::observer& obs, void 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::time t,
const irt::observer::status s); const irt::observer::status s);
......
...@@ -15,8 +15,8 @@ ...@@ -15,8 +15,8 @@
#include "node-editor.hpp" #include "node-editor.hpp"
#include <chrono> #include <chrono>
#include <fstream>
#include <filesystem> #include <filesystem>
#include <fstream>
#include <string> #include <string>
#include <fmt/format.h> #include <fmt/format.h>
...@@ -27,33 +27,83 @@ ...@@ -27,33 +27,83 @@
namespace irt { 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 void
plot_output::operator()(const irt::observer& obs, 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::time t,
const irt::observer::status s) const irt::observer::status s)
{ {
switch (s) { if (s == irt::observer::status::initialize) {
case irt::observer::status::initialize:
xs.clear(); xs.clear();
ys.clear(); ys.clear();
xs.reserve(4096u); xs.reserve(4096u * 4096u);
ys.reserve(4096u); ys.reserve(4096u * 4096u);
tl = t; return;
break; }
case irt::observer::status::run: {
const float value = static_cast<float>(obs.msg[0]);
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) { for (auto to_fill = tl; to_fill < t; to_fill += time_step) {
ys.emplace_back(value); const auto e = to_fill - tl;
xs.emplace_back(static_cast<float>(t)); 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;
tl = t; case irt::dynamics_type::qss2_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);
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; } break;
case irt::observer::status::finalize:
ys.emplace_back(static_cast<float>(obs.msg[0])); case irt::dynamics_type::qss3_integrator: {
xs.emplace_back(static_cast<float>(t)); 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;
default:
push_data(xs, ys, t, obs.msg[0]);
break; break;
} }
} }
...@@ -61,6 +111,7 @@ plot_output::operator()(const irt::observer& obs, ...@@ -61,6 +111,7 @@ plot_output::operator()(const irt::observer& obs,
void void
file_output::operator()(const irt::observer& obs, file_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::time t,
const irt::observer::status s) const irt::observer::status s)
{ {
...@@ -68,8 +119,6 @@ file_output::operator()(const irt::observer& obs, ...@@ -68,8 +119,6 @@ file_output::operator()(const irt::observer& obs,
switch (s) { switch (s) {
case irt::observer::status::initialize: case irt::observer::status::initialize:
tl = t;
if (ed && !ed->observation_directory.empty()) if (ed && !ed->observation_directory.empty())
file = ed->observation_directory; file = ed->observation_directory;
...@@ -83,7 +132,6 @@ file_output::operator()(const irt::observer& obs, ...@@ -83,7 +132,6 @@ file_output::operator()(const irt::observer& obs,
case irt::observer::status::run: case irt::observer::status::run:
if (ofs.is_open()) if (ofs.is_open())
fmt::print(ofs, "{},{}\n", t, obs.msg[0]); fmt::print(ofs, "{},{}\n", t, obs.msg[0]);
tl = t;
break; break;
case irt::observer::status::finalize: case irt::observer::status::finalize:
if (ofs.is_open()) { if (ofs.is_open()) {
......
...@@ -35,6 +35,7 @@ struct file_output ...@@ -35,6 +35,7 @@ struct file_output
void operator()(const irt::observer& obs, void 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::time t,
const irt::observer::status s) noexcept const irt::observer::status s) noexcept
{ {
......
...@@ -35,6 +35,7 @@ struct file_output ...@@ -35,6 +35,7 @@ struct file_output
void operator()(const irt::observer& obs, void 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::time t,
const irt::observer::status s) const irt::observer::status s)
{ {
......
...@@ -35,6 +35,7 @@ struct file_output ...@@ -35,6 +35,7 @@ struct file_output
void operator()(const irt::observer& obs, void 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::time t,
const irt::observer::status s) const irt::observer::status s)
{ {
......
...@@ -2478,7 +2478,7 @@ struct observer ...@@ -2478,7 +2478,7 @@ struct observer
using update_fn = using update_fn =
function_ref<void(const observer&, const dynamics_type, 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 observer(const char* name_, update_fn cb_) noexcept
: cb(cb_) : cb(cb_)
...@@ -2486,7 +2486,6 @@ struct observer ...@@ -2486,7 +2486,6 @@ struct observer
{} {}
update_fn cb; update_fn cb;
double tl = 0.0;
small_string<8> name; small_string<8> name;
model_id model = static_cast<model_id>(0); model_id model = static_cast<model_id>(0);
message msg; message msg;
...@@ -6478,8 +6477,7 @@ public: ...@@ -6478,8 +6477,7 @@ public:
while (observers.next(obs)) { while (observers.next(obs)) {
if (auto* mdl = models.try_to_get(obs->model); mdl) { if (auto* mdl = models.try_to_get(obs->model); mdl) {
obs->msg.reset(); obs->msg.reset();
obs->cb(*obs, mdl->type, t, observer::status::initialize); obs->cb(*obs, mdl->type, mdl->tl, t, observer::status::initialize);
obs->tl = t;
} }
} }
...@@ -6559,6 +6557,17 @@ public: ...@@ -6559,6 +6557,17 @@ public:
time t, time t,
flat_list<output_port_id>& emitting_output_ports) noexcept 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 (mdl.tn == mdl.handle->tn) {
if constexpr (is_detected_v<lambda_function_t, Dynamics>) { if constexpr (is_detected_v<lambda_function_t, Dynamics>) {
if constexpr (is_detected_v<has_output_port_t, Dynamics>) { if constexpr (is_detected_v<has_output_port_t, Dynamics>) {
...@@ -6584,18 +6593,6 @@ public: ...@@ -6584,18 +6593,6 @@ public:
if (auto* port = input_ports.try_to_get(dyn.x[i]); port) if (auto* port = input_ports.try_to_get(dyn.x[i]); port)
port->messages.clear(); 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); irt_assert(mdl.tn >= t);
mdl.tl = t; mdl.tl = t;
...@@ -6628,8 +6625,7 @@ public: ...@@ -6628,8 +6625,7 @@ public:
{ {
if constexpr (is_detected_v<observation_function_t, Dynamics>) { if constexpr (is_detected_v<observation_function_t, Dynamics>) {
obs.msg = dyn.observation(t - mdl.tl); obs.msg = dyn.observation(t - mdl.tl);
obs.cb(obs, mdl.type, t, observer::status::finalize); obs.cb(obs, mdl.type, mdl.tl, t, observer::status::finalize);
obs.tl = t;
} }
} }
......
...@@ -25,6 +25,7 @@ struct file_output ...@@ -25,6 +25,7 @@ struct file_output
void operator()(const irt::observer& obs, void 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::time t,
const irt::observer::status s) const irt::observer::status s)
{ {
......
...@@ -34,6 +34,7 @@ struct file_output ...@@ -34,6 +34,7 @@ struct file_output
void operator()(const irt::observer& obs, void 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::time t,
const irt::observer::status s) noexcept 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