diff --git a/CMakeLists.txt b/CMakeLists.txt index b306cb7..2c446d6 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/pybind11 b/pybind11 index 0bd8896..ed466da 160000 --- a/pybind11 +++ b/pybind11 @@ -1 +1 @@ -Subproject commit 0bd8896a4010f2d91b2340570c24fa08606ec406 +Subproject commit ed466da571fbc1d711351eb818e4bf82adb99eca diff --git a/python/python_bindings.cpp b/python/python_bindings.cpp index acf2859..3dd4332 100644 --- a/python/python_bindings.cpp +++ b/python/python_bindings.cpp @@ -95,20 +95,20 @@ PYBIND11_MODULE(usv_libs_py, m){ .def("update", &DynamicModel::update) .def("currentState", &DynamicModel::currentState); - py::class_(model, "DynamicModelOutput") + py::class_(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(); o.pose_y = t[1].cast(); o.pose_psi = t[2].cast(); diff --git a/src/control/datatypes.h b/src/control/datatypes.h index a423dad..bc6a0d3 100644 --- a/src/control/datatypes.h +++ b/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}; + }; } \ No newline at end of file diff --git a/src/model/dynamic_model.cpp b/src/model/dynamic_model.cpp index a0570ec..58a4094 100644 --- a/src/model/dynamic_model.cpp +++ b/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); diff --git a/src/model/dynamic_model.h b/src/model/dynamic_model.h index 71155cd..bc2bcaa 100644 --- a/src/model/dynamic_model.h +++ b/src/model/dynamic_model.h @@ -6,18 +6,16 @@ #define USV_ROS2_DYNAMIC_MODEL_H #include +#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{}; }; diff --git a/src/perturbances/wind_and_waves.cpp b/src/perturbances/wind_and_waves.cpp new file mode 100644 index 0000000..0c8f613 --- /dev/null +++ b/src/perturbances/wind_and_waves.cpp @@ -0,0 +1,86 @@ +#include "wind_and_waves.h" + +WindAndWaves::WindAndWaves(const WindAndWavesParams ¶ms) { + this->p = params; + dist_wF = std::normal_distribution(p.mean_wF, p.stddev_wF); + dist_wN = std::normal_distribution(p.mean_wN, p.stddev_wN); + dist_dF = std::normal_distribution(p.mean_dF, p.stddev_dF); + dist_dN = std::normal_distribution(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; +} \ No newline at end of file diff --git a/src/perturbances/wind_and_waves.h b/src/perturbances/wind_and_waves.h new file mode 100644 index 0000000..50b4c26 --- /dev/null +++ b/src/perturbances/wind_and_waves.h @@ -0,0 +1,69 @@ +#pragma once +#include +#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 ¶ms = 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 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; +}; \ No newline at end of file diff --git a/src/utils/ControllerUtils.cpp b/src/utils/ControllerUtils.cpp index c76d4bc..f60a280 100644 --- a/src/utils/ControllerUtils.cpp +++ b/src/utils/ControllerUtils.cpp @@ -1,13 +1,13 @@ #include "ControllerUtils.h" -std::pair ControllerUtils::update(DynamicModel &model, ASMC &controller, const ASMCSetpoint &setpoint) { +std::pair 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> +std::pair, std::vector> 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(n); @@ -16,7 +16,7 @@ ControllerUtils::update_n(DynamicModel &model, ASMC &controller, ASMCSetpoint se double acceleration = setpoint.velocity_setpoint / static_cast(n); setpoint.velocity_setpoint = model.currentState().u; - std::vector modelOutput(n); + std::vector modelOutput(n); std::vector asmcOutput(n); for(int i = 0; i < n; i++){ diff --git a/src/utils/ControllerUtils.h b/src/utils/ControllerUtils.h index e5ea055..e88e91a 100644 --- a/src/utils/ControllerUtils.h +++ b/src/utils/ControllerUtils.h @@ -6,11 +6,11 @@ namespace ControllerUtils { // Updates controller with dynamic model, for use in simulation - std::pair update(DynamicModel &model, ASMC &controller, const ASMCSetpoint &setpoint); + std::pair 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> update_n(DynamicModel &model, ASMC &controller, ASMCSetpoint setpoint, int n); + std::pair, std::vector> update_n(DynamicModel &model, ASMC &controller, ASMCSetpoint setpoint, int n); } \ No newline at end of file diff --git a/test/asmc_test.cpp b/test/asmc_test.cpp index c8dd49e..a361356 100644 --- a/test/asmc_test.cpp +++ b/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 modelStates; + std::vector modelStates; std::vector asmcOutputs; std::tie(modelStates, asmcOutputs) = ControllerUtils::update_n(model, controller, setpoint, n); diff --git a/test/dynamic_model_test.cpp b/test/dynamic_model_test.cpp index 1c3b2d3..95383d7 100644 --- a/test/dynamic_model_test.cpp +++ b/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); } diff --git a/test/wind_and_waves_test.cpp b/test/wind_and_waves_test.cpp new file mode 100644 index 0000000..41e2cc8 --- /dev/null +++ b/test/wind_and_waves_test.cpp @@ -0,0 +1,21 @@ +#include +#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); +} \ No newline at end of file