Merge branch 'master' of https://github.com/vanttec/usv_libs

CMakeLists.txt

@@ -14,11 +14,11 @@ FetchContent_MakeAvailable(googletest)
 find_package (Eigen3 3.3 REQUIRED NO_MODULE)
 
 set(PYBIND11_FINDPYTHON ON)
-#find_package (Python3 COMPONENTS Interpreter Development)
+find_package (Python3 COMPONENTS Interpreter Development)
 add_subdirectory(pybind11)
 
 set(CMAKE_CXX_STANDARD 17)
-set(USV_LIBS_SRC src/control/ASMC.cpp src/control/AITSMC.cpp src/model/dynamic_model.cpp src/utils/ControllerUtils.cpp)
+set(USV_LIBS_SRC src/control/ASMC.cpp src/control/AITSMC.cpp src/model/dynamic_model.cpp src/utils/ControllerUtils.cpp src/perturbances/wind_and_waves.cpp)
 
 # Library compilation
 include_directories(src)

python/python_bindings.cpp

@@ -95,20 +95,20 @@ PYBIND11_MODULE(usv_libs_py, m){
           .def("update", &DynamicModel::update)
           .def("currentState", &DynamicModel::currentState);
 
-  py::class_<DynamicModelOutput>(model, "DynamicModelOutput")
+  py::class_<ModelState>(model, "ModelState")
           .def(py::init<>())
-          .def_readwrite("pose_x", &DynamicModelOutput::pose_x)
-          .def_readwrite("pose_y", &DynamicModelOutput::pose_y)
-          .def_readwrite("pose_psi", &DynamicModelOutput::pose_psi)
-          .def_readwrite("u", &DynamicModelOutput::u)
-          .def_readwrite("v", &DynamicModelOutput::v)
-          .def_readwrite("r", &DynamicModelOutput::r)
+          .def_readwrite("pose_x", &ModelState::pose_x)
+          .def_readwrite("pose_y", &ModelState::pose_y)
+          .def_readwrite("pose_psi", &ModelState::pose_psi)
+          .def_readwrite("u", &ModelState::u)
+          .def_readwrite("v", &ModelState::v)
+          .def_readwrite("r", &ModelState::r)
           .def(py::pickle(
-                  [](const DynamicModelOutput &o){
+                  [](const ModelState &o){
                     return py::make_tuple(o.pose_x, o.pose_y, o.pose_psi, o.u, o.v, o.r);
                   },
                   [](py::tuple t) {
-                    DynamicModelOutput o;
+                    ModelState o;
                     o.pose_x = t[0].cast<double>();
                     o.pose_y = t[1].cast<double>();
                     o.pose_psi = t[2].cast<double>();

src/control/datatypes.h

@@ -37,4 +37,9 @@ namespace vanttec{
     }
 
   };
+
+  struct ModelState{
+    double pose_x{0}, pose_y{0}, pose_psi{0};
+    double u{0}, v{0}, r{0};
+  };
 }

src/model/dynamic_model.cpp

@@ -15,7 +15,7 @@ DynamicModel::DynamicModel() : DynamicModel(0, 0, 0) {
   ;
 }
 
-DynamicModelOutput DynamicModel::update(double leftThruster, double rightThruster) {
+ModelState DynamicModel::update(double leftThruster, double rightThruster) {
   double Xu = -25;
   double Xuu = 0;
   auto upsilon_abs = upsilon.cwiseAbs();
@@ -74,7 +74,7 @@ DynamicModelOutput DynamicModel::update(double leftThruster, double rightThruste
   eta = integral_step * (eta_dot + eta_dot_last) / 2 + eta;  // integral
   eta_dot_last = eta_dot;
 
-  DynamicModelOutput out{};
+  ModelState out{};
   out.pose_x = eta(0);
   out.pose_y = eta(1);
   out.pose_psi = eta(2);

src/model/dynamic_model.h

@@ -6,18 +6,16 @@
 #define USV_ROS2_DYNAMIC_MODEL_H
 
 #include <Eigen/Dense>
+#include "control/datatypes.h"
 
-struct DynamicModelOutput{
-  double pose_x{0}, pose_y{0}, pose_psi{0};
-  double u{0}, v{0}, r{0};
-};
+using namespace vanttec;
 
 class DynamicModel {
 public:
   DynamicModel();
   DynamicModel(double pose_x, double pose_y, double pose_psi);
-  DynamicModelOutput update(double leftThruster, double rightThruster);
-  [[nodiscard]] const DynamicModelOutput& currentState() const {
+  ModelState update(double leftThruster, double rightThruster);
+  [[nodiscard]] const ModelState& currentState() const {
     return state;
   }
 
@@ -51,7 +49,7 @@ class DynamicModel {
   Eigen::Vector3f eta_dot_last = Eigen::Vector3f::Zero();
   Eigen::Matrix3f M;
 
-  DynamicModelOutput state{};
+  ModelState state{};
 };
 
 

src/perturbances/wind_and_waves.cpp

@@ -0,0 +1,86 @@
+#include "wind_and_waves.h"
+
+WindAndWaves::WindAndWaves(const WindAndWavesParams &params) {
+  this->p = params;
+  dist_wF = std::normal_distribution<double>(p.mean_wF, p.stddev_wF);
+  dist_wN = std::normal_distribution<double>(p.mean_wN, p.stddev_wN);
+  dist_dF = std::normal_distribution<double>(p.mean_dF, p.stddev_dF);
+  dist_dN = std::normal_distribution<double>(p.mean_dN, p.stddev_dN);
+}
+
+ModelState WindAndWaves::update(const vanttec::ModelState &state) {
+  double wF = dist_wF(gen);
+  double wN = dist_wN(gen);
+  double dF_dot = dist_dF(gen);
+  double dN_dot = dist_dN(gen);
+
+  double U = std::hypot(state.u, state.v);
+  double we = std::abs(w0 - (w0 * w0 / g) * U * std::cos(p.beta_wave));
+  double xF2_dot = -we * we * xF1 - 2 * lambda * we * xF2 + Kw * wF;
+  xF2 = p.integral_step * (xF2_dot + xF2_dot_last) / 2 + xF2;
+  xF2_dot_last = xF2_dot;
+  double xF1_dot = xF2;
+  xF1 = p.integral_step * (xF1_dot + xF1_dot_last) / 2 + xF1;
+  xF1_dot_last = xF1_dot;
+
+  dF = p.integral_step * (dF_dot + dF_dot_last) / 2 + dF;
+  dF_dot_last = dF_dot;
+
+  double F_wave = (xF2 + dF) * p.scale_factor;
+
+  double xN2_dot = -we * we * xN1 - 2 * lambda * we * xN2 + Kw * wN;
+  xN2 = p.integral_step * (xN2_dot + xN2_dot_last) / 2 + xN2;
+  xN2_dot_last = xN2_dot;
+  double xN1_dot = xN2;
+  xN1 = p.integral_step * (xN1_dot + xN1_dot_last) / 2 + xN1;
+  xN1_dot_last = xN1_dot;
+
+  dN = p.integral_step * (dN_dot + dN_dot_last) / 2 + dN;
+  dN_dot_last = dN_dot;
+
+  double N_wave = (xN2 + dN) * p.scale_factor;
+
+  double X_wave = F_wave * std::cos(p.beta_wave - state.pose_psi);
+  double Y_wave = F_wave * std::sin(p.beta_wave - state.pose_psi);
+
+  double u_w = p.V_wind * std::cos(p.beta_wind - state.pose_psi);
+  double v_w = p.V_wind * std::sin(p.beta_wind - state.pose_psi);
+
+  double u_rw = state.u - u_w;
+  double v_rw = state.v - v_w;
+
+  double gamma_rw = std::atan2(v_rw, u_rw);
+  double V_rw = std::hypot(u_rw, v_rw);
+
+  double CDlaf;
+  if (std::abs(gamma_rw) > 1.5708) {
+    CDlaf = CDlaf_pi;
+  } else {
+    CDlaf = CDlaf_0;
+  }
+  double CDl = CDlaf * AFW / ALW;
+
+  double CX = CDlaf * std::cos(gamma_rw) /
+              (1 - ((delta_wind / 2) * (1 - (CDl / CDt)) * (std::sin(2 * gamma_rw) * std::sin(2 * gamma_rw))));
+  double CY = CDt * std::sin(gamma_rw) /
+              (1 - ((delta_wind / 2) * (1 - (CDl / CDt)) * (std::sin(2 * gamma_rw) * std::sin(2 * gamma_rw))));
+  double CN = -0.18 * (gamma_rw - M_PI_2) * CY;
+
+  double X_wind = 0.5 * rho * V_rw * V_rw * CX * AFW;
+  double Y_wind = 0.5 * rho * V_rw * V_rw * CY * ALW;
+  double N_wind = 0.5 * rho * V_rw * V_rw * CN * AFW * LOA;
+
+  double delta_x = X_wave + X_wind;
+  double delta_y = Y_wave + Y_wind;
+  double delta_theta = N_wave + N_wind;;
+
+  ModelState outState = state;
+  outState.u = 0;
+  outState.v = 0;
+  outState.r = 0;
+  outState.pose_x += delta_x;
+  outState.pose_y += delta_y;
+  outState.pose_psi += delta_theta;
+
+  return outState;
+}

src/perturbances/wind_and_waves.h

@@ -0,0 +1,69 @@
+#pragma once
+#include <random>
+#include "control/datatypes.h"
+using namespace vanttec;
+
+struct WindAndWavesParams {
+  double beta_wave;
+  double mean_wF, mean_dF;
+  double mean_wN, mean_dN;
+  double stddev_wF, stddev_dF;
+  double stddev_wN, stddev_dN;
+  double beta_wind;
+  double V_wind;
+  double scale_factor;
+  double integral_step{0.01};
+
+  static WindAndWavesParams defaultParams() {
+    WindAndWavesParams p;
+    p.beta_wave = 0.0;
+    p.stddev_wF = 1.0;
+    p.stddev_dF = 0.5;
+    p.stddev_wN = 0.2;
+    p.stddev_dN = 0.1;
+    p.mean_wF = 0;
+    p.mean_dF = 0;
+    p.mean_wN = 0;
+    p.mean_dN = 0;
+    p.beta_wind = 0.0;
+    p.V_wind = 5.11;
+    p.scale_factor = 1.0;
+    p.integral_step = 0.01;
+    return p;
+  }
+};
+
+class WindAndWaves{
+public:
+  WindAndWaves(const WindAndWavesParams &params = WindAndWavesParams::defaultParams());
+
+  /*
+   * Takes in current state, returns perturbed pose
+   */
+  ModelState update(const ModelState &state);
+private:
+  WindAndWavesParams p;
+  std::default_random_engine gen;
+  std::normal_distribution<double> dist_wF, dist_wN, dist_dF, dist_dN;
+
+  double xN1{0}, xN2{0};
+  double xF1{0}, xF2{0};
+  double dF{0}, dN{0};
+  double xF1_dot_last{0}, xF2_dot_last{0};
+  double xN1_dot_last{0}, xN2_dot_last{0};
+  double dF_dot_last{0}, dN_dot_last{0};
+
+  constexpr static double w0 = 0.8;
+  constexpr static double lambda = 0.1;
+  constexpr static double Kw = 0.64;
+  constexpr static double g = 9.81;
+
+  constexpr static double rho = 1.0;
+  constexpr static double CDlaf_0 = 0.55;
+  constexpr static double CDlaf_pi = 0.6;
+  constexpr static double CDt = 0.9;
+  constexpr static double AFW = 0.045;
+  constexpr static double ALW = 0.09;
+  constexpr static double LOA = 0.9;
+  constexpr static double delta_wind = 0.6;
+};

src/utils/ControllerUtils.cpp

@@ -1,13 +1,13 @@
 #include "ControllerUtils.h"
 
-std::pair<DynamicModelOutput, ASMCOutput> ControllerUtils::update(DynamicModel &model, ASMC &controller, const ASMCSetpoint &setpoint) {
+std::pair<ModelState, ASMCOutput> ControllerUtils::update(DynamicModel &model, ASMC &controller, const ASMCSetpoint &setpoint) {
   auto state = fromModelASMC(model);
   auto asmcOut = controller.update(state, setpoint);
   auto modelOut = model.update(asmcOut.left_thruster, asmcOut.right_thruster);
   return {modelOut, asmcOut};
 }
 
-std::pair<std::vector<DynamicModelOutput>, std::vector<ASMCOutput>>
+std::pair<std::vector<ModelState>, std::vector<ASMCOutput>>
 ControllerUtils::update_n(DynamicModel &model, ASMC &controller, ASMCSetpoint setpoint, int n) {
   // Use heading setpoint struct as angular vel, use velocity as acceleration TESTING TESTING
   double angularVel = setpoint.heading_setpoint / static_cast<double>(n);
@@ -16,7 +16,7 @@ ControllerUtils::update_n(DynamicModel &model, ASMC &controller, ASMCSetpoint se
   double acceleration = setpoint.velocity_setpoint / static_cast<double>(n);
   setpoint.velocity_setpoint = model.currentState().u;
 
-  std::vector<DynamicModelOutput> modelOutput(n);
+  std::vector<ModelState> modelOutput(n);
   std::vector<ASMCOutput> asmcOutput(n);
 
   for(int i = 0; i < n; i++){

src/utils/ControllerUtils.h

@@ -6,11 +6,11 @@
 
 namespace ControllerUtils {
   // Updates controller with dynamic model, for use in simulation
-  std::pair<DynamicModelOutput, ASMCOutput> update(DynamicModel &model, ASMC &controller, const ASMCSetpoint &setpoint);
+  std::pair<ModelState, ASMCOutput> update(DynamicModel &model, ASMC &controller, const ASMCSetpoint &setpoint);
   ASMCState fromModelASMC(const DynamicModel &model);
   vanttec::ControllerState fromModel(const DynamicModel &model);
 
   // Used to run asmc and model n times, speeds up python bindings
   // NOTE, setpoint heading is used as an angular velocity input :)
-  std::pair<std::vector<DynamicModelOutput>, std::vector<ASMCOutput>> update_n(DynamicModel &model, ASMC &controller, ASMCSetpoint setpoint, int n);
+  std::pair<std::vector<ModelState>, std::vector<ASMCOutput>> update_n(DynamicModel &model, ASMC &controller, ASMCSetpoint setpoint, int n);
 }

test/asmc_test.cpp

@@ -37,7 +37,7 @@ TEST_F(ASMCTest, UpdateNReachesVelAndHeading){
   //Janky code, this is actuall ang vel, and accel
   setpoint.heading_setpoint = M_PI_2;
   setpoint.velocity_setpoint = 0.5;
-  std::vector<DynamicModelOutput> modelStates;
+  std::vector<ModelState> modelStates;
   std::vector<ASMCOutput> asmcOutputs;
   std::tie(modelStates, asmcOutputs) = ControllerUtils::update_n(model, controller, setpoint, n);
 

test/dynamic_model_test.cpp

@@ -14,7 +14,7 @@ class DynamicModelTest : public ::testing::Test {
 
 TEST_F(DynamicModelTest, StaticTest){
   //Boat should stay relatively still when no thruster output is applied
-  DynamicModelOutput out{};
+  ModelState out{};
   for(int i = 0; i < n; i++){
     out = model.update(0, 0);
   }
@@ -29,7 +29,7 @@ TEST_F(DynamicModelTest, StaticTest){
 }
 
 TEST_F(DynamicModelTest, MoveForward){
-  DynamicModelOutput out{};
+  ModelState out{};
   for(int i = 0; i < n; i++){
     out = model.update(5, 5);
   }

test/wind_and_waves_test.cpp

@@ -0,0 +1,21 @@
+#include <gtest/gtest.h>
+#include "perturbances/wind_and_waves.h"
+
+class WindAndWavesTest : public ::testing::Test {
+protected:
+  void SetUp() override {
+    waves = WindAndWaves();
+  }
+  WindAndWaves waves;
+};
+
+TEST_F(WindAndWavesTest, SanityCheck){
+  //Check if wind and waves returns something ne 0
+  ModelState state;
+  state.u = 1;
+
+  ModelState perturb = waves.update(state);
+  EXPECT_NE(perturb.pose_x, 0);
+  EXPECT_NE(perturb.pose_y, 0);
+  EXPECT_NE(perturb.pose_psi, 0);
+}