Skip to content

Commit 1e028d7

Browse files
pockermanpockerman
authored
Merge pull request #172 from pockerman/feat/169_refactor_ctrl_lib_from_cuberl
Feat/169 refactor ctrl lib from cuberl
2 parents 8dfe408 + ec407be commit 1e028d7

30 files changed

+4614
-10
lines changed

‎CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -159,6 +159,8 @@ FILE(GLOB SRCS src/bitrl/*.cpp
159159
src/bitrl/envs/webots_envs/*.cpp
160160
src/bitrl/envs/gymnasium/box2d/*.cpp
161161
src/bitrl/boards/arduino/*.cpp
162+
src/bitrl/control/*.cpp
163+
src/bitrl/planning/*.cpp
162164
src/bitrl/rigid_bodies/*.cpp
163165
src/bitrl/rigid_bodies/chrono_robots/*.cpp
164166
src/bitrl/rigid_bodies/webots_robots/*.cpp

‎examples/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,8 @@ ADD_SUBDIRECTORY(example_12)
3838
ADD_SUBDIRECTORY(example_14)
3939
ADD_SUBDIRECTORY(example_15)
4040
ADD_SUBDIRECTORY(example_16)
41+
ADD_SUBDIRECTORY(example_21)
42+
ADD_SUBDIRECTORY(example_22)
4143

4244
IF(BITRL_MQTT)
4345
ADD_SUBDIRECTORY(example_17)

‎examples/example_1/example_1.md‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,10 +5,10 @@ specifically how to create an interact with <a href="https://gymnasium.farama.or
55

66
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>.
77
bitrl itself implements classes that hide this interaction from the client code.
8-
In general, environment classes in bitrl, have to implement the \ref bitrl::envs::EnvBase "`bitrl::envs::EnvBase`" API.
8+
In general, environment classes in bitrl, have to implement the \ref bitrl::envs::EnvBase "bitrl::envs::EnvBase" API.
99

10-
In this example we will use the \ref bitrl::envs::gymnasium::FrozenLake "`bitrl::envs::gymnasium::FrozenLake`"
11-
class. This is a template class, see the example below, that itself inherits from \ref bitrl::envs::gymnasium::GymnasiumEnvBase "`bitrl::envs::gymnasium::GymnasiumEnvBase`"
10+
In this example we will use the \ref bitrl::envs::gymnasium::FrozenLake "bitrl::envs::gymnasium::FrozenLake"
11+
class. This is a template class, see the example below, that itself inherits from \ref bitrl::envs::gymnasium::GymnasiumEnvBase "bitrl::envs::gymnasium::GymnasiumEnvBase"
1212
class.
1313

1414
Below is the driver code.
@@ -132,6 +132,6 @@ int main()
132132
@endcode
133133

134134
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
135-
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`",
136-
\ref bitrl::envs::gymnasium::CliffWorld "`bitrl::envs::gymnasium::CliffWorld`" and \ref bitrl::envs::gymnasium::BlackJack "`bitrl::envs::gymnasium::BlackJack`"
135+
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",
136+
\ref bitrl::envs::gymnasium::CliffWorld "bitrl::envs::gymnasium::CliffWorld" and \ref bitrl::envs::gymnasium::BlackJack "bitrl::envs::gymnasium::BlackJack"
137137
environments.

‎examples/example_21/CMakeLists.txt‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
CMAKE_MINIMUM_REQUIRED(VERSION 3.20 FATAL_ERROR)
2+
3+
SET(EXECUTABLE example_21)
4+
SET(SOURCE ${EXECUTABLE}.cpp)
5+
6+
ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
7+
8+
TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log)
9+
10+
IF(ENABLE_CHRONO)
11+
TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core)
12+
ENDIF ()
13+
14+
IF(BITRL_WEBOTS)
15+
TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)
16+
ENDIF()
17+
18+
IF(BITRL_MQTT)
19+
TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as)
20+
ENDIF ()
21+
22+
IF(BITRL_OPENCV)
23+
TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${OpenCV_LIBS})
24+
ENDIF ()

‎examples/example_21/example_21.cpp‎

Lines changed: 195 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,195 @@
1+
/**
2+
* KalmanFilter example. The example is taken from the
3+
* paper <a href="https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf">An Introduction to the Kalman Filter</a> by
4+
* Greg Welch and Gary Bishop
5+
* */
6+
7+
#include "bitrl/bitrl_types.h"
8+
#include "bitrl/estimation/kalman_filter.h"
9+
#include "bitrl/estimation/kf_model_base.h"
10+
#include "bitrl/utils/iteration_counter.h"
11+
#include "bitrl/utils/io/csv_file_writer.h"
12+
13+
#define BOOST_LOG_DYN_LINK
14+
#include <boost/log/trivial.hpp>
15+
#include <iostream>
16+
#include <unordered_map>
17+
#include <any>
18+
#include <vector>
19+
#include <random>
20+
21+
namespace example_1
22+
{
23+
24+
using bitrl::real_t;
25+
using bitrl::uint_t;
26+
using bitrl::DynMat;
27+
using bitrl::DynVec;
28+
using bitrl::estimation::KalmanFilterConfig;
29+
using bitrl::estimation::KalmanFilter;
30+
using bitrl::estimation::KFMotionModelBase;
31+
using bitrl::estimation::KFModelBase;
32+
using bitrl::utils::IterationCounter;
33+
using bitrl::utils::io::CSVWriter;
34+
35+
36+
real_t DT = 1.0;
37+
real_t SIM_TIME = 50.0;
38+
39+
struct Cmd
40+
{
41+
// v: [m/s] omega: [rad/s]
42+
static DynVec<real_t> cmd();
43+
};
44+
45+
DynVec<real_t>
46+
Cmd::cmd(){
47+
48+
DynVec<real_t> u(1);
49+
u << 0.0;
50+
return u;
51+
}
52+
53+
// simple struct that describes the Motion of the robots
54+
struct MotionModel: public KFMotionModelBase<DynMat<real_t>, DynVec<real_t>>
55+
{
56+
const static uint_t VEC_SIZE;
57+
58+
};
59+
60+
const uint_t MotionModel::VEC_SIZE = 1;
61+
62+
// simple struct that describes the Observation model
63+
struct ObservationModel: public KFModelBase<DynMat<real_t>>
64+
{
65+
const static uint_t VEC_SIZE;
66+
static DynVec<real_t> sensors();
67+
};
68+
69+
const uint_t ObservationModel::VEC_SIZE = 1;
70+
71+
const real_t STD = 0.1;
72+
const real_t MU = -0.37727;
73+
74+
DynVec<real_t>
75+
ObservationModel::sensors(){
76+
DynVec<real_t> all_sensors(1);
77+
78+
std::normal_distribution<real_t> d{MU , STD};
79+
std::mt19937 generator; //(42);
80+
all_sensors << d(generator);
81+
return all_sensors;
82+
}
83+
84+
85+
typedef MotionModel motion_model_type;
86+
typedef ObservationModel obs_motion_type;
87+
}
88+
89+
int main() {
90+
91+
using namespace example_1;
92+
93+
try{
94+
95+
typedef motion_model_type::state_type state_type;
96+
obs_motion_type obs;
97+
DynMat<real_t> H(ObservationModel::VEC_SIZE, MotionModel::VEC_SIZE);
98+
H << 1.0;
99+
100+
obs.add_matrix("H", H);
101+
102+
// the motion model
103+
motion_model_type motion;
104+
105+
// set the initial state
106+
DynVec<real_t> x_init(MotionModel::VEC_SIZE);
107+
x_init << 0;
108+
motion.set_state(x_init);
109+
110+
//...and the matrix describing the motion dynamics
111+
DynMat<real_t> F(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
112+
F << 1.0;
113+
114+
motion.add_matrix("F", F);
115+
116+
KalmanFilterConfig<motion_model_type, obs_motion_type> kf_config;
117+
kf_config.motion_model = &motion;
118+
kf_config.observation_model = &obs;
119+
120+
// the KalmanFilter to use
121+
KalmanFilter<motion_model_type, obs_motion_type> kf(kf_config);
122+
123+
// set up the matrices
124+
DynMat<real_t> P(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
125+
P << 1.0;
126+
127+
DynMat<real_t> Q(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
128+
Q << 1.0e-5;
129+
130+
DynMat<real_t> R(ObservationModel::VEC_SIZE, ObservationModel::VEC_SIZE);
131+
R << STD*STD;
132+
133+
DynMat<real_t> B(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
134+
B << 0.0;
135+
136+
kf.with_matrix("P", P)
137+
.with_matrix("Q", Q)
138+
.with_matrix("R", R)
139+
.with_matrix("B", B);
140+
141+
142+
// the input to the filter
143+
std::map<std::string, std::any > kf_input;
144+
145+
auto n_steps = static_cast<uint_t>(SIM_TIME / DT);
146+
147+
BOOST_LOG_TRIVIAL(info)<<"Expected number of time steps: "<<n_steps;
148+
std::vector<state_type> rows;
149+
rows.reserve(n_steps);
150+
151+
auto current_time = 0.0;
152+
while(current_time <= SIM_TIME){
153+
154+
auto u = Cmd::cmd();
155+
auto w = DynVec<real_t>(u.size());
156+
w << 0.0;
157+
auto z = ObservationModel::sensors();
158+
159+
kf_input["u"] = u;
160+
kf_input["w"] = w;
161+
kf_input["z"] = z;
162+
163+
auto& state_vec = kf.estimate(kf_input);
164+
165+
BOOST_LOG_TRIVIAL(info)<<"Time: "<<current_time<<" solution "<<state_vec[0];
166+
167+
state_type row(1 + state_vec.size());
168+
row[0] = current_time;
169+
170+
for(uint_t i =0; i < static_cast<uint_t>(state_vec.size()); ++i){
171+
row[i + 1] = state_vec[i];
172+
}
173+
174+
rows.push_back(row);
175+
current_time += DT;
176+
}
177+
178+
CSVWriter csv_writer("state.csv");
179+
csv_writer.open();
180+
181+
for(auto& r:rows){
182+
csv_writer.write_row(r);
183+
}
184+
185+
}
186+
catch(std::exception& e){
187+
std::cout<<e.what()<<std::endl;
188+
}
189+
catch(...){
190+
191+
std::cout<<"Unknown exception occured"<<std::endl;
192+
}
193+
194+
return 0;
195+
}

0 commit comments

Comments
 (0)