Added perturbs to model

vanttec · May 15, 2023 · 03ba369 · 03ba369
1 parent e3418aa
commit 03ba369
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 1 deletion.
diff --git a/python/python_bindings.cpp b/python/python_bindings.cpp
@@ -93,6 +93,7 @@ PYBIND11_MODULE(usv_libs_py, m){
           .def(py::init<>())
           .def(py::init<double,double,double>())
           .def("update", &DynamicModel::update)
+          .def("update_with_perturb", &DynamicModel::update_with_perturb)
           .def("currentState", &DynamicModel::currentState);
 
   py::class_<ModelState>(model, "ModelState")

diff --git a/src/model/dynamic_model.cpp b/src/model/dynamic_model.cpp
@@ -16,6 +16,11 @@ DynamicModel::DynamicModel() : DynamicModel(0, 0, 0) {
 }
 
 ModelState DynamicModel::update(double leftThruster, double rightThruster) {
+  return update_with_perturb(leftThruster, rightThruster, {0, 0, 0});
+}
+
+ModelState DynamicModel::update_with_perturb(double leftThruster, double rightThruster,
+                                             const std::array<double, 3> &T_perturb) {
   double Xu = -25;
   double Xuu = 0;
   auto upsilon_abs = upsilon.cwiseAbs();
@@ -34,7 +39,9 @@ ModelState DynamicModel::update(double leftThruster, double rightThruster) {
   double Nr = 0.02 * (-3.141592 * 1000) * vel * 0.09 * 0.09 * 1.01 * 1.01;
 
   Eigen::Vector3f T;
-  T << leftThruster + c * rightThruster, 0, 0.5 * B * (leftThruster - c * rightThruster);
+  T << leftThruster + c * rightThruster + T_perturb[0],
+        T_perturb[1],
+        0.5 * B * (leftThruster - c * rightThruster) + T_perturb[2];
 
   Eigen::Matrix3f CRB, CA, C;
   CRB << 0, 0, 0 - m * upsilon(1), 0, 0, m * upsilon(0), m * upsilon(1),

diff --git a/src/model/dynamic_model.h b/src/model/dynamic_model.h
@@ -15,6 +15,7 @@ class DynamicModel {
   DynamicModel();
   DynamicModel(double pose_x, double pose_y, double pose_psi);
   ModelState update(double leftThruster, double rightThruster);
+  ModelState update_with_perturb(double leftThruster, double rightThruster, const std::array<double, 3> &T_perturb);
   [[nodiscard]] const ModelState& currentState() const {
     return state;
   }