Feat/169 refactor ctrl lib from cuberl

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

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)

examples/example_1/example_1.md

@@ -5,10 +5,10 @@ specifically how to create an interact with <a href="https://gymnasium.farama.or
 
 In bitrl, Gymnasium-based environment are interacted over a REST-like API maintained here: <a href="https://github.com/pockerman/bitrl-rest-api">bitrl-envs-api</a>.
 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 <a href="https://github.com/pockerman/bitrl-rest-api">bitrl-envs-api</a> 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.

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 ()

examples/example_21/example_21.cpp

@@ -0,0 +1,195 @@
+/**
+  * KalmanFilter example. The example is taken from the 
+  * paper <a href="https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf">An Introduction to the Kalman Filter</a> 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 <boost/log/trivial.hpp>
+#include <iostream>
+#include <unordered_map>
+#include <any>
+#include <vector>
+#include <random>
+
+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<real_t> cmd();
+};
+
+DynVec<real_t> 
+Cmd::cmd(){
+
+	DynVec<real_t> u(1);
+	u << 0.0;
+    return u;
+}
+
+// simple struct that describes the Motion of the robots
+struct MotionModel: public KFMotionModelBase<DynMat<real_t>, DynVec<real_t>>
+{
+	const static uint_t VEC_SIZE;
+
+};
+
+const uint_t MotionModel::VEC_SIZE = 1;
+
+// simple struct that describes the Observation model
+struct ObservationModel: public KFModelBase<DynMat<real_t>>
+{
+	const static uint_t VEC_SIZE;
+	static DynVec<real_t> sensors();
+};
+
+const uint_t ObservationModel::VEC_SIZE = 1;
+
+const real_t STD = 0.1;
+const real_t MU = -0.37727;
+
+DynVec<real_t> 
+ObservationModel::sensors(){ 
+	DynVec<real_t> all_sensors(1);
+
+	std::normal_distribution<real_t> 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<real_t> 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<real_t> x_init(MotionModel::VEC_SIZE);
+		x_init << 0;
+		motion.set_state(x_init);
+
+		//...and  the matrix describing the motion dynamics
+		DynMat<real_t> F(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+		F << 1.0;
+
+		motion.add_matrix("F", F);
+
+		KalmanFilterConfig<motion_model_type, obs_motion_type> kf_config;
+		kf_config.motion_model = &motion;
+		kf_config.observation_model = &obs;
+
+		// the KalmanFilter to use
+		KalmanFilter<motion_model_type, obs_motion_type> kf(kf_config);
+
+		// set up the matrices
+		DynMat<real_t> P(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+		P << 1.0;
+
+		DynMat<real_t> Q(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+		Q << 1.0e-5;
+
+		DynMat<real_t> R(ObservationModel::VEC_SIZE, ObservationModel::VEC_SIZE);
+		R << STD*STD;
+
+		DynMat<real_t> 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<std::string, std::any > kf_input; 
+
+		auto n_steps = static_cast<uint_t>(SIM_TIME / DT);
+
+		BOOST_LOG_TRIVIAL(info)<<"Expected number of time steps: "<<n_steps;
+		std::vector<state_type> rows;
+		rows.reserve(n_steps);
+
+		auto current_time = 0.0;
+		while(current_time <= SIM_TIME){
+
+			auto u = Cmd::cmd();
+			auto w = DynVec<real_t>(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: "<<current_time<<" solution "<<state_vec[0];
+
+			state_type row(1 + state_vec.size());
+			row[0] = current_time;
+
+			for(uint_t i =0; i < static_cast<uint_t>(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<<e.what()<<std::endl;
+    }
+    catch(...){
+
+        std::cout<<"Unknown exception occured"<<std::endl;
+    }
+
+   return 0;
+}