diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8ea6fba..69392ba 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -159,6 +159,8 @@ FILE(GLOB SRCS src/bitrl/*.cpp
src/bitrl/envs/webots_envs/*.cpp
src/bitrl/envs/gymnasium/box2d/*.cpp
src/bitrl/boards/arduino/*.cpp
+ src/bitrl/control/*.cpp
+ src/bitrl/planning/*.cpp
src/bitrl/rigid_bodies/*.cpp
src/bitrl/rigid_bodies/chrono_robots/*.cpp
src/bitrl/rigid_bodies/webots_robots/*.cpp
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 44129de..e4b146e 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -38,6 +38,8 @@ ADD_SUBDIRECTORY(example_12)
ADD_SUBDIRECTORY(example_14)
ADD_SUBDIRECTORY(example_15)
ADD_SUBDIRECTORY(example_16)
+ADD_SUBDIRECTORY(example_21)
+ADD_SUBDIRECTORY(example_22)
IF(BITRL_MQTT)
ADD_SUBDIRECTORY(example_17)
diff --git a/examples/example_1/example_1.md b/examples/example_1/example_1.md
index 2d10c5b..f49c39e 100644
--- a/examples/example_1/example_1.md
+++ b/examples/example_1/example_1.md
@@ -5,10 +5,10 @@ specifically how to create an interact with bitrl-envs-api.
bitrl itself implements classes that hide this interaction from the client code.
-In general, environment classes in bitrl, have to implement the \ref bitrl::envs::EnvBase "`bitrl::envs::EnvBase`" API.
+In general, environment classes in bitrl, have to implement the \ref bitrl::envs::EnvBase "bitrl::envs::EnvBase" API.
-In this example we will use the \ref bitrl::envs::gymnasium::FrozenLake "`bitrl::envs::gymnasium::FrozenLake`"
-class. This is a template class, see the example below, that itself inherits from \ref bitrl::envs::gymnasium::GymnasiumEnvBase "`bitrl::envs::gymnasium::GymnasiumEnvBase`"
+In this example we will use the \ref bitrl::envs::gymnasium::FrozenLake "bitrl::envs::gymnasium::FrozenLake"
+class. This is a template class, see the example below, that itself inherits from \ref bitrl::envs::gymnasium::GymnasiumEnvBase "bitrl::envs::gymnasium::GymnasiumEnvBase"
class.
Below is the driver code.
@@ -132,6 +132,6 @@ int main()
@endcode
In order to run the example you will need an instance of the bitrl-envs-api server running
-on your machine listening at por 8001. Note the actual example also shows how to use \ref bitrl::envs::gymnasium::Taxi "`bitrl::envs::gymnasium::Taxi`",
-\ref bitrl::envs::gymnasium::CliffWorld "`bitrl::envs::gymnasium::CliffWorld`" and \ref bitrl::envs::gymnasium::BlackJack "`bitrl::envs::gymnasium::BlackJack`"
+on your machine listening at por 8001. Note the actual example also shows how to use \ref bitrl::envs::gymnasium::Taxi "bitrl::envs::gymnasium::Taxi",
+\ref bitrl::envs::gymnasium::CliffWorld "bitrl::envs::gymnasium::CliffWorld" and \ref bitrl::envs::gymnasium::BlackJack "bitrl::envs::gymnasium::BlackJack"
environments.
diff --git a/examples/example_21/CMakeLists.txt b/examples/example_21/CMakeLists.txt
new file mode 100644
index 0000000..ebc023f
--- /dev/null
+++ b/examples/example_21/CMakeLists.txt
@@ -0,0 +1,24 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20 FATAL_ERROR)
+
+SET(EXECUTABLE example_21)
+SET(SOURCE ${EXECUTABLE}.cpp)
+
+ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
+
+TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log)
+
+IF(ENABLE_CHRONO)
+ TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core)
+ENDIF ()
+
+IF(BITRL_WEBOTS)
+ TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)
+ENDIF()
+
+IF(BITRL_MQTT)
+ TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as)
+ENDIF ()
+
+IF(BITRL_OPENCV)
+ TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${OpenCV_LIBS})
+ENDIF ()
diff --git a/examples/example_21/example_21.cpp b/examples/example_21/example_21.cpp
new file mode 100755
index 0000000..d0c68e4
--- /dev/null
+++ b/examples/example_21/example_21.cpp
@@ -0,0 +1,195 @@
+/**
+ * KalmanFilter example. The example is taken from the
+ * paper An Introduction to the Kalman Filter by
+ * Greg Welch and Gary Bishop
+ * */
+
+#include "bitrl/bitrl_types.h"
+#include "bitrl/estimation/kalman_filter.h"
+#include "bitrl/estimation/kf_model_base.h"
+#include "bitrl/utils/iteration_counter.h"
+#include "bitrl/utils/io/csv_file_writer.h"
+
+#define BOOST_LOG_DYN_LINK
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace example_1
+{
+
+using bitrl::real_t;
+using bitrl::uint_t;
+using bitrl::DynMat;
+using bitrl::DynVec;
+using bitrl::estimation::KalmanFilterConfig;
+using bitrl::estimation::KalmanFilter;
+using bitrl::estimation::KFMotionModelBase;
+using bitrl::estimation::KFModelBase;
+using bitrl::utils::IterationCounter;
+using bitrl::utils::io::CSVWriter;
+
+
+real_t DT = 1.0;
+real_t SIM_TIME = 50.0;
+
+struct Cmd
+{
+ // v: [m/s] omega: [rad/s]
+ static DynVec cmd();
+};
+
+DynVec
+Cmd::cmd(){
+
+ DynVec u(1);
+ u << 0.0;
+ return u;
+}
+
+// simple struct that describes the Motion of the robots
+struct MotionModel: public KFMotionModelBase, DynVec>
+{
+ const static uint_t VEC_SIZE;
+
+};
+
+const uint_t MotionModel::VEC_SIZE = 1;
+
+// simple struct that describes the Observation model
+struct ObservationModel: public KFModelBase>
+{
+ const static uint_t VEC_SIZE;
+ static DynVec sensors();
+};
+
+const uint_t ObservationModel::VEC_SIZE = 1;
+
+const real_t STD = 0.1;
+const real_t MU = -0.37727;
+
+DynVec
+ObservationModel::sensors(){
+ DynVec all_sensors(1);
+
+ std::normal_distribution d{MU , STD};
+ std::mt19937 generator; //(42);
+ all_sensors << d(generator);
+ return all_sensors;
+}
+
+
+typedef MotionModel motion_model_type;
+typedef ObservationModel obs_motion_type;
+}
+
+int main() {
+
+ using namespace example_1;
+
+ try{
+
+ typedef motion_model_type::state_type state_type;
+ obs_motion_type obs;
+ DynMat H(ObservationModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ H << 1.0;
+
+ obs.add_matrix("H", H);
+
+ // the motion model
+ motion_model_type motion;
+
+ // set the initial state
+ DynVec x_init(MotionModel::VEC_SIZE);
+ x_init << 0;
+ motion.set_state(x_init);
+
+ //...and the matrix describing the motion dynamics
+ DynMat F(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ F << 1.0;
+
+ motion.add_matrix("F", F);
+
+ KalmanFilterConfig kf_config;
+ kf_config.motion_model = &motion;
+ kf_config.observation_model = &obs;
+
+ // the KalmanFilter to use
+ KalmanFilter kf(kf_config);
+
+ // set up the matrices
+ DynMat P(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ P << 1.0;
+
+ DynMat Q(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ Q << 1.0e-5;
+
+ DynMat R(ObservationModel::VEC_SIZE, ObservationModel::VEC_SIZE);
+ R << STD*STD;
+
+ DynMat B(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ B << 0.0;
+
+ kf.with_matrix("P", P)
+ .with_matrix("Q", Q)
+ .with_matrix("R", R)
+ .with_matrix("B", B);
+
+
+ // the input to the filter
+ std::map kf_input;
+
+ auto n_steps = static_cast(SIM_TIME / DT);
+
+ BOOST_LOG_TRIVIAL(info)<<"Expected number of time steps: "< rows;
+ rows.reserve(n_steps);
+
+ auto current_time = 0.0;
+ while(current_time <= SIM_TIME){
+
+ auto u = Cmd::cmd();
+ auto w = DynVec(u.size());
+ w << 0.0;
+ auto z = ObservationModel::sensors();
+
+ kf_input["u"] = u;
+ kf_input["w"] = w;
+ kf_input["z"] = z;
+
+ auto& state_vec = kf.estimate(kf_input);
+
+ BOOST_LOG_TRIVIAL(info)<<"Time: "<(state_vec.size()); ++i){
+ row[i + 1] = state_vec[i];
+ }
+
+ rows.push_back(row);
+ current_time += DT;
+ }
+
+ CSVWriter csv_writer("state.csv");
+ csv_writer.open();
+
+ for(auto& r:rows){
+ csv_writer.write_row(r);
+ }
+
+ }
+ catch(std::exception& e){
+ std::cout<An Introduction to the Kalman Filter by
+Greg Welch and Gary Bishop. In fact the implementation of the ```KalmaFilter``` follows this paper.
+
+
+
+## The driver code
+
+```
+/**
+ * KalmanFilter example.
+ * */
+
+#include "cubeai/base/cubeai_types.h"
+#include "cubeai/estimation/kalman_filter.h"
+#include "cubeai/estimation/kf_model_base.h"
+#include "cubeai/utils/iteration_counter.h"
+#include "cubeai/io/csv_file_writer.h"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace example_1
+{
+
+using cubeai::real_t;
+using cubeai::uint_t;
+using cubeai::DynMat;
+using cubeai::DynVec;
+using cubeai::estimation::KalmanFilterConfig;
+using cubeai::estimation::KalmanFilter;
+using cubeai::estimation::KFMotionModelBase;
+using cubeai::estimation::KFModelBase;
+using cubeai::utils::IterationCounter;
+using cubeai::io::CSVWriter;
+
+
+real_t DT = 1.0;
+real_t SIM_TIME = 50.0;
+
+struct Cmd
+{
+ // v: [m/s] omega: [rad/s]
+ static DynVec cmd();
+};
+
+DynVec
+Cmd::cmd(){
+
+ DynVec u(1);
+ u << 0.0;
+ return u;
+}
+
+// simple struct that describes the Motion of the robots
+struct MotionModel: public KFMotionModelBase, DynVec>
+{
+ const static uint_t VEC_SIZE;
+
+};
+
+const uint_t MotionModel::VEC_SIZE = 1;
+
+// simple struct that describes the Observation model
+struct ObservationModel: public KFModelBase>
+{
+ const static uint_t VEC_SIZE;
+ static DynVec sensors();
+};
+
+const uint_t ObservationModel::VEC_SIZE = 1;
+
+const real_t STD = 0.1;
+const real_t MU = -0.37727;
+
+DynVec
+ObservationModel::sensors(){
+ DynVec all_sensors(1);
+
+ std::normal_distribution d{MU , STD};
+ std::mt19937 generator; //(42);
+ all_sensors << d(generator);
+ return all_sensors;
+}
+
+
+typedef MotionModel motion_model_type;
+typedef ObservationModel obs_motion_type;
+}
+
+int main() {
+
+ using namespace example_1;
+
+ try{
+
+ typedef motion_model_type::state_type state_type;
+ obs_motion_type obs;
+ DynMat H(ObservationModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ H << 1.0;
+
+ obs.add_matrix("H", H);
+
+ // the motion model
+ motion_model_type motion;
+
+ // set the initial state
+ DynVec x_init(MotionModel::VEC_SIZE);
+ x_init << 0;
+ motion.set_state(x_init);
+
+ //...and the matrix describing the motion dynamics
+ DynMat F(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ F << 1.0;
+
+ motion.add_matrix("F", F);
+
+ KalmanFilterConfig kf_config;
+ kf_config.motion_model = &motion;
+ kf_config.observation_model = &obs;
+
+ // the KalmanFilter to use
+ KalmanFilter kf(kf_config);
+
+ // set up the matrices
+ DynMat P(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ P << 1.0;
+
+ DynMat Q(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ Q << 1.0e-5;
+
+ DynMat R(ObservationModel::VEC_SIZE, ObservationModel::VEC_SIZE);
+ R << STD*STD;
+
+ DynMat B(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+ B << 0.0;
+
+ kf.with_matrix("P", P)
+ .with_matrix("Q", Q)
+ .with_matrix("R", R)
+ .with_matrix("B", B);
+
+
+ // the input to the filter
+ std::map kf_input;
+
+ auto n_steps = static_cast(SIM_TIME / DT);
+
+ BOOST_LOG_TRIVIAL(info)<<"Expected number of time steps: "< rows;
+ rows.reserve(n_steps);
+
+ auto current_time = 0.0;
+ while(current_time <= SIM_TIME){
+
+ auto u = Cmd::cmd();
+ auto w = DynVec(u.size());
+ w << 0.0;
+ auto z = ObservationModel::sensors();
+
+ kf_input["u"] = u;
+ kf_input["w"] = w;
+ kf_input["z"] = z;
+
+ auto& state_vec = kf.estimate(kf_input);
+
+ BOOST_LOG_TRIVIAL(info)<<"Time: "<(state_vec.size()); ++i){
+ row[i + 1] = state_vec[i];
+ }
+
+ rows.push_back(row);
+ current_time += DT;
+ }
+
+ CSVWriter csv_writer("state.csv");
+ csv_writer.open();
+
+ for(auto& r:rows){
+ csv_writer.write_row(r);
+ }
+
+ }
+ catch(std::exception& e){
+ std::cout<
+#include
+#include
+#include
+#include
+#include
+
+namespace example_2
+{
+
+using bitrl::real_t;
+using bitrl::uint_t;
+using bitrl::DynMat;
+using bitrl::DynVec;
+using bitrl::estimation::ExtendedKalmanFilter;
+
+using bitrl::utils::io::CSVWriter;
+using bitrl::utils::IterationCounter;
+using bitrl::dynamics::DiffDriveDynamics;
+using bitrl::dynamics::SysState;
+
+
+class ObservationModel
+{
+
+public:
+
+ // the ExtendedKalmanFilter expects an exposed
+ // input_type
+ typedef DynVec input_type;
+
+ ObservationModel();
+
+ // simply return the state
+ const DynVec evaluate(const DynVec& input)const;
+
+ // get the H or M matrix
+ const DynMat& get_matrix(const std::string& name)const;
+
+private:
+
+ DynMat H;
+ DynMat M;
+};
+
+ObservationModel::ObservationModel()
+ :
+ H(2, 3),
+ M(2, 2)
+{
+ H = DynMat::Zero(2,3);
+ M = DynMat::Zero(2,2);
+ H(0, 0) = 1.0;
+ H(1,1) = 1.0;
+ M(0,0) = 1.0;
+ M(1, 1) = 1.0;
+
+}
+
+const DynVec
+ObservationModel::evaluate(const DynVec& input)const{
+ return input;
+}
+
+const DynMat&
+ObservationModel::get_matrix(const std::string& name)const{
+ if(name == "H"){
+ return H;
+ }
+ else if(name == "M"){
+ return M;
+ }
+
+ throw std::logic_error("Invalid matrix name. Name "+name+ " not found");
+}
+
+const DynVec get_measurement(const SysState<3>& state){
+
+ DynVec result(2);
+ result[0] = state.get("X");
+ result[1] = state.get("Y");
+ return result;
+}
+
+
+}
+
+int main() {
+
+ using namespace example_2;
+
+ BOOST_LOG_TRIVIAL(info)<<"Starting example...";
+
+ uint_t n_steps = 300;
+ auto time = 0.0;
+ auto dt = 0.5;
+
+ /// angular velocity
+ auto w = 0.0;
+
+ /// linear velocity
+ auto vt = 1.0;
+
+ std::array motion_control_error;
+ motion_control_error[0] = 0.0;
+ motion_control_error[1] = 0.0;
+
+ DiffDriveDynamics exact_motion_model;
+ exact_motion_model.set_matrix_update_flag(false);
+ exact_motion_model.set_time_step(dt);
+
+ DiffDriveDynamics motion_model;
+ motion_model.set_time_step(dt);
+
+ std::map input;
+ input["v"] = 1.0;
+ input["w"] = 0.0;
+ input["errors"] = motion_control_error;
+ motion_model.initialize_matrices(input);
+
+ ObservationModel observation;
+
+ ExtendedKalmanFilter ekf(motion_model, observation);
+
+ DynMat R = DynMat::Zero(2, 2);
+ R(0,0) = 1.0;
+ R(1, 1) = 1.0;
+
+ DynMat Q = DynMat::Zero(2, 2);
+ Q(0,0) = 0.001;
+ Q(1, 1) = 0.001;
+
+ DynMat P = DynMat::Zero(3, 3);
+ P(0, 0) = 1.0;
+ P(1, 1) = 1.0;
+ P(2, 2) = 1.0;
+
+ ekf.with_matrix("P", P)
+ .with_matrix("R", R)
+ .with_matrix("Q", Q);
+
+ CSVWriter writer("state");
+ writer.open();
+ std::vector names{"Time", "X_true", "Y_true", "Theta_true", "X", "Y", "Theta"};
+ writer.write_column_names(names);
+
+ try{
+
+
+ BOOST_LOG_TRIVIAL(info)<<"Starting simulation... "< motion_input;
+ motion_input["v"] = vt; // we keep the velocity constant
+ motion_input["errors"] = motion_control_error;
+
+ for(uint_t step=0; step < n_steps; ++step){
+
+ BOOST_LOG_TRIVIAL(info)<<"Simulation time: "< row(7, 0.0);
+ row[0] = time;
+ row[1] = exact_state.get("X");
+ row[2] = exact_state.get("Y");
+ row[3] = exact_state.get("Theta");
+ row[4] = state.get("X");
+ row[5] = state.get("Y");
+ row[6] = state.get("Theta");
+ writer.write_row(row);
+
+ time += dt;
+ counter++;
+ }
+
+ BOOST_LOG_TRIVIAL(info)<<"Finished example...";
+ }
+ catch(std::runtime_error& e){
+ std::cerr<
+#include
+#include
+#include
+#include
+#include
+
+namespace example_2
+{
+
+using cubeai::real_t;
+using cubeai::uint_t;
+using cubeai::DynMat;
+using cubeai::DynVec;
+using cubeai::estimation::ExtendedKalmanFilter;
+using cubeai::utils::IterationCounter;
+using cubeai::io::CSVWriter;
+using rlenvscpp::dynamics::DiffDriveDynamics;
+using rlenvscpp::dynamics::SysState;
+
+
+class ObservationModel
+{
+
+public:
+
+ // the ExtendedKalmanFilter expects an exposed
+ // input_type
+ typedef DynVec input_type;
+
+ ObservationModel();
+
+ // simply return the state
+ const DynVec evaluate(const DynVec& input)const;
+
+ // get the H or M matrix
+ const DynMat& get_matrix(const std::string& name)const;
+
+private:
+
+ DynMat H;
+ DynMat M;
+};
+
+ObservationModel::ObservationModel()
+ :
+ H(2, 3),
+ M(2, 2)
+{
+ H = DynMat::Zero(2,3);
+ M = DynMat::Zero(2,2);
+ H(0, 0) = 1.0;
+ H(1,1) = 1.0;
+ M(0,0) = 1.0;
+ M(1, 1) = 1.0;
+
+}
+
+const DynVec
+ObservationModel::evaluate(const DynVec& input)const{
+ return input;
+}
+
+const DynMat&
+ObservationModel::get_matrix(const std::string& name)const{
+ if(name == "H"){
+ return H;
+ }
+ else if(name == "M"){
+ return M;
+ }
+
+ throw std::logic_error("Invalid matrix name. Name "+name+ " not found");
+}
+
+const DynVec get_measurement(const SysState<3>& state){
+
+ DynVec result(2);
+ result[0] = state.get("X");
+ result[1] = state.get("Y");
+ return result;
+}
+
+
+}
+
+int main() {
+
+ using namespace example_2;
+
+ BOOST_LOG_TRIVIAL(info)<<"Starting example...";
+
+ uint_t n_steps = 300;
+ auto time = 0.0;
+ auto dt = 0.5;
+
+ /// angular velocity
+ auto w = 0.0;
+
+ /// linear velocity
+ auto vt = 1.0;
+
+ std::array motion_control_error;
+ motion_control_error[0] = 0.0;
+ motion_control_error[1] = 0.0;
+
+ DiffDriveDynamics exact_motion_model;
+ exact_motion_model.set_matrix_update_flag(false);
+ exact_motion_model.set_time_step(dt);
+
+ DiffDriveDynamics motion_model;
+ motion_model.set_time_step(dt);
+
+ std::map input;
+ input["v"] = 1.0;
+ input["w"] = 0.0;
+ input["errors"] = motion_control_error;
+ motion_model.initialize_matrices(input);
+
+ ObservationModel observation;
+
+ ExtendedKalmanFilter ekf(motion_model, observation);
+
+ DynMat R = DynMat::Zero(2, 2);
+ R(0,0) = 1.0;
+ R(1, 1) = 1.0;
+
+ DynMat Q = DynMat::Zero(2, 2);
+ Q(0,0) = 0.001;
+ Q(1, 1) = 0.001;
+
+ DynMat P = DynMat::Zero(3, 3);
+ P(0, 0) = 1.0;
+ P(1, 1) = 1.0;
+ P(2, 2) = 1.0;
+
+ ekf.with_matrix("P", P)
+ .with_matrix("R", R)
+ .with_matrix("Q", Q);
+
+ CSVWriter writer("state");
+ writer.open();
+ std::vector names{"Time", "X_true", "Y_true", "Theta_true", "X", "Y", "Theta"};
+ writer.write_column_names(names);
+
+ try{
+
+
+ BOOST_LOG_TRIVIAL(info)<<"Starting simulation... "< motion_input;
+ motion_input["v"] = vt; // we keep the velocity constant
+ motion_input["errors"] = motion_control_error;
+
+ for(uint_t step=0; step < n_steps; ++step){
+
+ BOOST_LOG_TRIVIAL(info)<<"Simulation time: "< row(7, 0.0);
+ row[0] = time;
+ row[1] = exact_state.get("X");
+ row[2] = exact_state.get("Y");
+ row[3] = exact_state.get("Theta");
+ row[4] = state.get("X");
+ row[5] = state.get("Y");
+ row[6] = state.get("Theta");
+ writer.write_row(row);
+
+ time += dt;
+ counter++;
+ }
+
+ BOOST_LOG_TRIVIAL(info)<<"Finished example...";
+ }
+ catch(std::runtime_error& e){
+ std::cerr<
+
+#include
+#include