diff --git a/config/invictasim/config.yaml b/config/invictasim/config.yaml index c9444817c..2b09b4f16 100644 --- a/config/invictasim/config.yaml +++ b/config/invictasim/config.yaml @@ -1,4 +1,4 @@ invictasim: # Simulator config - vehicle_model: "bicycle_model" # Options: "bicycle_model" + vehicle_model: "bicycle_model" # Options: "bicycle_model", "FSFEUP02_model" timestep: 0.001 # seconds (0.001s = 1000 Hz) diff --git a/config/invictasim/vehicle_models/FSFEUP02_model.yaml b/config/invictasim/vehicle_models/FSFEUP02_model.yaml new file mode 100644 index 000000000..e2db85f38 --- /dev/null +++ b/config/invictasim/vehicle_models/FSFEUP02_model.yaml @@ -0,0 +1,31 @@ +vehicle_model: + FSFEUP02_model: + # Mass and inertia + sm: 162.0 # sprung mass without driver (assuming driver of 70kg) -> conchinha + um: 38.0 # unsprung mass + Izz: 120.0 # Yaw moment of inertia (kg·m²) + cg: + # Sprung CG Y and Z axis in mm + sCGy: 722.0 + sCGz: 263.0 + #Unsprung mass CG Y and Z axis in mm + uCGy: 744.0 + uCGz: 206.0 + # %Total mass CG + # CGy=(sCGy*sm+uCGy*um)/m; + CGz: 252.2 + # Drivetrain + dimensions: # in mm + wheelbase: 1530 + track_front: 1200 + track_rear: 1200 + tire: + model: "r2016x7.5r10x8" #Options: "r2016x7.5r10x8" + #IMPORTANT: THESE PARAMETERS ARE WRONG BECAUSE THE CG IS NOT CORRECT WITHOUT THE DRIVER CHANGE WHEN THERE IS ACTUAL DATA + d_bright: 1.06 + d_bleft: 1.06 + d_fright: 0.99 + d:fleft: 0.99 + camber: 0 + gearRatio: 3.7 + \ No newline at end of file diff --git a/config/invictasim/vehicle_models/tire_models/r2016x7.5r10x8.yaml b/config/invictasim/vehicle_models/tire_models/r2016x7.5r10x8.yaml new file mode 100644 index 000000000..ae8af7447 --- /dev/null +++ b/config/invictasim/vehicle_models/tire_models/r2016x7.5r10x8.yaml @@ -0,0 +1,292 @@ +dimension: + UNLOADED_RADIUS: 0.235 + WIDTH: 0.2159 + ASPECT_RATIO: 0.55 + RIM_RADIUS: 0.1905 + RIM_WIDTH: 0.152 + +operating_conditions: + INFLPRES: 220000 + NOMPRES: 220000 + +inertia: + MASS: 17.451 + IXX: 0.4 + IYY: 0.7 + BELT_MASS: 7 + BELT_IXX: 0.34 + BELT_IYY: 0.6 + GRAVITY: -9.81 + +vertical: + FNOMIN: 1113.5001 + VERTICAL_STIFFNESS: 200000 + VERTICAL_DAMPING: 50 + MC_CONTOUR_A: 0.5 + MC_CONTOUR_B: 0.5 + BREFF: 8.4 + DREFF: 0.27 + FREFF: 0.07 + Q_RE0: 1 + Q_V1: 0 + Q_V2: 0 + Q_FZ2: 0.0001 + Q_FCX: 0 + Q_FCY: 0 + Q_CAM: 0 + PFZ1: 0.88241 + Q_FCY2: -0.4751 + Q_CAM1: 85.19 + Q_CAM2: 257.4 + Q_CAM3: 0.5119 + Q_FYS1: -20496.4 + Q_FYS2: -60000 + Q_FYS3: 88211.7 + BOTTOM_OFFST: 0.01 + BOTTOM_STIFF: 2000000 + +structural: + LONGITUDINAL_STIFFNESS: 300000 + LATERAL_STIFFNESS: 100000 + YAW_STIFFNESS: 5000 + FREQ_LONG: 80 + FREQ_LAT: 40 + FREQ_YAW: 50 + FREQ_WINDUP: 70 + DAMP_LONG: 0.04 + DAMP_LAT: 0.04 + DAMP_YAW: 0.04 + DAMP_WINDUP: 0.04 + DAMP_RESIDUAL: 0.002 + DAMP_VLOW: 0.001 + Q_BVX: 0 + Q_BVT: 0 + PCFX1: 0 + PCFX2: 0 + PCFX3: 0 + PCFY1: 0 + PCFY2: 0 + PCFY3: 0 + PCMZ1: 0 + +contact_patch: + Q_RA1: 0.5 + Q_RA2: 1 + Q_RB1: 1 + Q_RB2: -1 + ELLIPS_SHIFT: 0.8 + ELLIPS_LENGTH: 1 + ELLIPS_HEIGHT: 1 + ELLIPS_ORDER: 1.8 + ELLIPS_MAX_STEP: 0.025 + ELLIPS_NWIDTH: 10 + ELLIPS_NLENGTH: 10 + +inflation_pressure_range: + PRESMIN: 10000 + PRESMAX: 1000000 + +vertical_force_range: + FZMIN: 228.6 + FZMAX: 1600 + +long_slip_range: + KPUMIN: -0.25 + KPUMAX: 0.25 + +slip_angle_range: + ALPMIN: -0.2094 + ALPMAX: 0.2094 + +inclination_angle_range: + CAMMIN: -0.1047 + CAMMAX: 0.1047 + +scaling_coefficients: + LFZO: 1 + LCX: 1 + LMUX: 1 + LEX: 1 + LKX: 1 + LHX: 0 + LVX: 0 + LCY: 1 + LMUY: 1 + LEY: 1 + LKY: 1 + LHY: 0 + LVY: 0 + LTR: 1 + LRES: 0 + LXAL: 1 + LYKA: 1 + LVYKA: 1 + LS: 1 + LKYC: 1 + LKZC: 1 + LVMX: 0 + LMX: 1 + LMY: 1 + LMP: 1 + +longitudinal_coefficients: + PCX1: 1.786 + PDX1: 2.933 + PDX2: -0.44 + PDX3: 20.8 + PEX1: 0.871 + PEX2: -0.038 + PEX3: 0 + PEX4: 0.071 + PKX1: 85.31 + PKX2: -20.25 + PKX3: 0.5 + PHX1: 0 + PHX2: 0 + PVX1: 0 + PVX2: 0 + PPX1: 0 + PPX2: 0 + PPX3: 0 + PPX4: 0 + RBX1: 23.72 + RBX2: 25.97 + RBX3: 0 + RCX1: 0.7495 + REX1: -0.4759 + REX2: 0.8109 + RHX1: 0 + +overturning_coefficients: + QSX1: 0 + QSX2: 0 + QSX3: 0 + QSX4: 5 + QSX5: 1 + QSX6: 10 + QSX7: 0 + QSX8: 0 + QSX9: 0.4 + QSX10: 0 + QSX11: 5 + QSX12: 0 + QSX13: 0 + QSX14: 0 + PPMX1: 0 + +lateral_coefficients: + PCY1: 1.6384 + PDY1: 2.3749 + PDY2: -0.1985 + PDY3: 4.0072 + PEY1: 0.53284 + PEY2: -0.33312 + PEY3: 0.27748 + PEY4: -0.016493 + PEY5: 0 + PKY1: -38.9068 + PKY2: 1.6336 + PKY3: 0.62971 + PKY4: 2 + PKY5: 0 + PKY6: -1 + PKY7: 0 + PHY1: -4.3588e-16 + PHY2: -2.2117e-17 + PVY1: 9.4249e-16 + PVY2: 3.9982e-18 + PVY3: -3.2711 + PVY4: -1.1442 + PPY1: 0 + PPY2: 0 + PPY3: 0 + PPY4: 0 + PPY5: 0 + RBY1: 20.33 + RBY2: 8.152 + RBY3: -0.01243 + RBY4: 0 + RCY1: 0.9317 + REY1: -0.0003982 + REY2: 0.3077 + RHY1: 1.3384e-17 + RHY2: 1.352e-17 + RVY1: 2.0035e-17 + RVY2: -1.3167e-17 + RVY3: 2.2302e-18 + RVY4: -6.7562e-18 + RVY5: 7.583e-18 + RVY6: 2.0886e-17 + +rolling_coefficients: + QSY1: -0.0309 + QSY2: -0.0921 + QSY3: 0 + QSY4: 0 + QSY5: 0 + QSY6: 0 + QSY7: 0.85 + QSY8: -0.4 + +aligning_coefficients: + QBZ1: 10 + QBZ2: 0 + QBZ3: 0 + QBZ4: 0 + QBZ5: 0 + QBZ9: 10 + QBZ10: 0 + QCZ1: 1.1 + QDZ1: 0.12 + QDZ2: 0 + QDZ3: 0 + QDZ4: 0 + QDZ6: 0 + QDZ7: 0 + QDZ8: -0.05 + QDZ9: 0 + QDZ10: 0 + QDZ11: 0 + QEZ1: 0 + QEZ2: 0 + QEZ3: 0 + QEZ4: 0 + QEZ5: 0 + QHZ1: 0 + QHZ2: 0 + QHZ3: 0 + QHZ4: 0 + PPZ1: 0 + PPZ2: 0 + SSZ1: 0 + SSZ2: 0 + SSZ3: 0 + SSZ4: 0 + + # POSTA DO CHAT ATE TER DADOS DE VD +turnslip_coefficients: + PECP1: 0.8 + PECP2: 0.1 + PDXP1: 0.5 + PDXP2: 0.0 + PDXP3: 1.0 + PDXP4: 0.0 + PDYP1: 1.0 + PDYP2: 0.1 + PDYP3: 1.0 + PDYP4: 0.0 + PKYP1: 35 + PHYP1: 0.2 + PHYP2: 0.1 + PHYP3: 0.3 + PHYP4: 0.0 + + # Other configuration numbers +configurations: + Amu: 1.0 + + + + + + \ No newline at end of file diff --git a/src/invictasim/CMakeLists.txt b/src/invictasim/CMakeLists.txt index ec5f797f7..fcd24691e 100644 --- a/src/invictasim/CMakeLists.txt +++ b/src/invictasim/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 3.8) project(invictasim) -# Set C++ standard to 17 and required compiler options +# ===================== +# Compiler setup +# ===================== set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -9,7 +11,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +# ===================== +# Dependencies +# ===================== find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) @@ -26,52 +30,103 @@ find_package(fs_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(custom_interfaces REQUIRED) -set(IMPLEMENTATION_FILES - src/node/invictasim_node.cpp +# ===================== +# Core library (shared code) +# ===================== +add_library(invictasim_core src/config/config_loader.cpp src/vehicle_model/bicycle_model.cpp + src/vehicle_model/FSFEUP02_model.cpp + src/vehicle_model/tire_model/tire_model.cpp +) + +ament_target_dependencies(invictasim_core + rclcpp + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + rosgraph_msgs + tf2 + tf2_ros + yaml-cpp + common_lib + eufs_msgs + fs_msgs + visualization_msgs + custom_interfaces ) -add_executable(invictasim src/main.cpp ${IMPLEMENTATION_FILES}) -ament_target_dependencies(invictasim - rclcpp std_msgs geometry_msgs sensor_msgs nav_msgs rosgraph_msgs tf2 tf2_ros - yaml-cpp common_lib eufs_msgs fs_msgs visualization_msgs custom_interfaces) -target_include_directories(invictasim PUBLIC +target_include_directories(invictasim_core PUBLIC $ - $) -target_link_libraries(invictasim yaml-cpp) -target_compile_features(invictasim PUBLIC c_std_99 cxx_std_17) + $ +) + +# ===================== +# Main executable +# ===================== +add_executable(invictasim + src/main.cpp + src/node/invictasim_node.cpp +) + +target_link_libraries(invictasim + invictasim_core + yaml-cpp +) install(TARGETS invictasim - DESTINATION lib/${PROJECT_NAME}) + DESTINATION lib/${PROJECT_NAME} +) +# ===================== +# Tests +# ===================== if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) - set(TESTFILES + ament_add_gtest(${PROJECT_NAME}_test test/main.cpp test/test.cpp ) - include_directories(test) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}) - ament_add_gtest(${PROJECT_NAME}_test ${TESTFILES}) - ament_target_dependencies(${PROJECT_NAME}_test - rclcpp std_msgs geometry_msgs sensor_msgs nav_msgs tf2 tf2_ros - eufs_msgs fs_msgs visualization_msgs custom_interfaces) - target_include_directories(${PROJECT_NAME}_test PUBLIC - $ - $) - - install(TARGETS ${PROJECT_NAME}_test DESTINATION lib/${PROJECT_NAME}) + target_link_libraries(${PROJECT_NAME}_test + invictasim_core + yaml-cpp + ) + + install(TARGETS ${PROJECT_NAME}_test + DESTINATION lib/${PROJECT_NAME} + ) endif() -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) -install(DIRECTORY include/ DESTINATION include) +# ===================== +# Install & export +# ===================== +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + ament_export_include_directories(include) ament_export_dependencies( - rclcpp std_msgs geometry_msgs sensor_msgs nav_msgs rosgraph_msgs tf2 tf2_ros - yaml-cpp common_lib eufs_msgs fs_msgs visualization_msgs custom_interfaces) + rclcpp + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + rosgraph_msgs + tf2 + tf2_ros + yaml-cpp + common_lib + eufs_msgs + fs_msgs + visualization_msgs + custom_interfaces +) -ament_package() \ No newline at end of file +ament_package() diff --git a/src/invictasim/include/vehicle_model/FSFEUP02_model.hpp b/src/invictasim/include/vehicle_model/FSFEUP02_model.hpp new file mode 100644 index 000000000..f4723ee9d --- /dev/null +++ b/src/invictasim/include/vehicle_model/FSFEUP02_model.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include + +#include +#include + +#include "vd_helpers.hpp" +#include "vehicle_model/tire_model/tire_model.hpp" +#include "vehicle_model/vehicle_model.hpp" + +/** + * @brief Four wheel vehicle model (tuned for FSFEUP02) + */ +class FSFEUP02Model : public VehicleModel { +public: + explicit FSFEUP02Model(const std::string& config_path); + ~FSFEUP02Model() override = default; + void step(double dt) override; + void reset() override; + + double get_position_x() const override; + double get_position_y() const override; + double get_yaw() const override; + double get_velocity_x() const override; + + void set_position(double x, double y, double yaw) override; + void set_velocity(double vx) override; + void set_steering(double angle) override; + void set_throttle(double throttle) override; + + std::string get_model_name() const override; + float get_tire_effective_radius() const; + + // CHANGE PARAMETERS ARE STILL FROM BICYCLE MODEL +private: + VehicleModelParams params; + VehicleModelState state; + // Aerodynamics + double cla_; + double cda_; + double aero_area_; + + // Mass and inertia + double mass_; + double Izz_; + + // Drivetrain + double wheel_radius_; + double gear_ratio_; + + // State variables + double x_; + double y_; + double yaw_; + double vx_; + + // Control inputs + double steering_angle_; + double throttle_; + + // Tires + std::unique_ptr front_left; + std::unique_ptr front_right; + std::unique_ptr back_left; + std::unique_ptr back_right; +}; diff --git a/src/invictasim/include/vehicle_model/tire_model/tire_model.hpp b/src/invictasim/include/vehicle_model/tire_model/tire_model.hpp new file mode 100644 index 000000000..7420a53ca --- /dev/null +++ b/src/invictasim/include/vehicle_model/tire_model/tire_model.hpp @@ -0,0 +1,134 @@ +#pragma once +#include "../vd_helpers.hpp" + +struct TireInput { + Velocity v; + float yawR; + float angSpeed; +}; + +/* +------------------------------------------------------------------------------------------- + Only struct to be declared here since it is suposed to be used only for the tire model +------------------------------------------------------------------------------------------- +*/ +struct InternalValues { + double distance_to_CG; + double camber_angle; + double epsilong; + double epsilon = 1e-6; // safety factor to avoid division by zero + double dfz; + double Fz0_prime; + double Vc_prime; + double phi; + double dpi = 0; // defaulted to 0 (ignore pressure changes) + double gamma_star; + double zeta2; + double Kya; + double zeta3; + double Vcx; + double zeta1; + double LMUY_prime; + double SVyg; +}; + +// Could add combined slip if needed +struct TireOutput { + double Fx; + double Fy; +}; + +/* + Given the proposed logic the tire calculation should: + if Vx > 3 m/s + - Compute slip angle (alpha) using: + - velocity Y, velocity X, yaw rate and distance from cg + - Compute slip ratio (Kappa) using: + - velocity X, Tire radius, angular speed + - Compute pacejka + if Vx <= 3 m/s - Use lugre + NOTE: Prevent division by zero on Vx + +*/ + +//------------------------------------------------------------------------------------------------------------------------------------------------ +// IMPORTANT: In case of wanting to account for pressure changes, there is currently no +// implementation, we always assume constant tire pressure +//------------------------------------------------------------------------------------------------------------------------------------------------ +class TireModel { +public: + explicit TireModel(float camber, float dist); + + /** + * @brief Calculates the resulting forces from tire input + * + * @param input velocity, yawRate and angular speed + * @return TireOutput + */ + TireOutput calculateForces() + const; // Maybe TireOutput should be passed and returned by reference? + + /** + * @brief Calculates slip angle using velocity and yawRate + * Normalizes v_x to prevent division by 0 + * @return Corresponds to arctan((v_y + yawR*distance)/v_x) + */ + float calculateSlipAngle() const; + + /** + * @brief Calculates the slip ration using velocity and angular speed + * Normalizes v_x to prevent division by 0 + * @return Corresponds to ((effectiveTireRadius * AngularSpeed) - v_x) / v_x + */ + float calculateSlipRatio() const; + + /** + * @brief Uses the pacejka magic formula to calculate either Fx or Fy depending on the input + * values + * + * @param B Stiffness + * @param C Shape + * @param D Peak value + * @param E Fall-off + * @param slip slip angle or slip ratio + * @param SV Vertical Shift + * @return float Either Fx or Fy depeding on inputs + */ + float calculatePacejka(float B, float C, float D, float E, float slip, float SV) const; + + // REMOVE + float get_tire_effective_radius() const; + + bool calculateTireState(float slip_angle, float slip_ratio); + + /** + * @brief Calculates the D parameter for the Fy calculation using pacejka MF (Peak) + * + * @return float The value of Dy + */ + float calculateDy() const; + + float calculateDx() const; + + float calculateCx() const; + + float calculateCy() const; + + float calculateBx(float Dx, float Cx) const; + + float calculateBy(float Dy, float Cy) const; + + float calculateEx(double slip_ratio) const; + + float calculateEy(double slip_angle) const; + + float calculateSVx() const; + + float calculateSVy() const; + +private: + /*Values*/ + InternalValues internal_vals; + VehicleModelState vehicle_model_state; + VehicleModelParams vehicle_model_params; +}; diff --git a/src/invictasim/include/vehicle_model/vd_helpers.hpp b/src/invictasim/include/vehicle_model/vd_helpers.hpp new file mode 100644 index 000000000..2d9fb29b1 --- /dev/null +++ b/src/invictasim/include/vehicle_model/vd_helpers.hpp @@ -0,0 +1,246 @@ +#pragma once + +/* + Helper structs to make code modular + */ +struct Velocity { + double vx; + double vy; +}; +struct Force { + double fx; + double fy; + double fz; +}; + +struct TireParameters { + // ---------------- configuration values + double Amu = 1.0; // VD uses 1 but the pacejka book suggests 10 + // ---------------- dimension (geometry) + double UNLOADED_RADIUS = 0.235; // Free tire radius + double WIDTH = 0.2159; // Section width + double ASPECT_RATIO = 0.55; // Sidewall ratio + double RIM_RADIUS = 0.1905; // Rim radius + double RIM_WIDTH = 0.152; // Rim width + + // ---------------- operating conditions + double INFLPRES = 220000; // Actual inflation pressure + double NOMPRES = 220000; // Nominal reference pressure + + // ---------------- inertia + double MASS = 17.451; // Tire mass + double IXX = 0.4; // Rotational inertia (spin) + double IYY = 0.7; // Lateral inertia + double BELT_MASS = 7; // Belt mass + double BELT_IXX = 0.34; // Belt spin inertia + double BELT_IYY = 0.6; // Belt lateral inertia + double GRAVITY = -9.81; // Gravity sign convention + + // ---------------- vertical (Fz behavior) + double FNOMIN = 1113.5001; // Nominal load + double VERTICAL_STIFFNESS = 200000; // Radial stiffness + double VERTICAL_DAMPING = 50; // Radial damping + double MC_CONTOUR_A = 0.5; // Contact shape factor + double MC_CONTOUR_B = 0.5; // Contact shape factor + double BREFF = 8.4; // Effective rolling radius B + double DREFF = 0.27; // Effective rolling radius D + double FREFF = 0.07; // Effective rolling radius F + double Q_RE0 = 1; // Rolling radius scaling + double Q_V1 = 0; // Speed effect on radius + double Q_V2 = 0; // Speed² effect on radius + double Q_FZ2 = 0.0001; // Load² effect + double Q_FCX = 0; // Fx influence on Fz + double Q_FCY = 0; // Fy influence on Fz + double Q_CAM = 0; // Camber influence on Fz + double PFZ1 = 0.88241; // Vertical force scaling + double Q_FCY2 = -0.4751; // Nonlinear Fy–Fz coupling + double Q_CAM1 = 85.19; // Camber stiffness term + double Q_CAM2 = 257.4; // Camber stiffness term + double Q_CAM3 = 0.5119; // Camber stiffness term + double Q_FYS1 = -20496.4; // Lateral deflection term + double Q_FYS2 = -60000; // Lateral deflection term + double Q_FYS3 = 88211.7; // Lateral deflection term + double BOTTOM_OFFST = 0.01; // Hard-stop offset + double BOTTOM_STIFF = 2000000; // Hard-stop stiffness + + // ---------------- structural (relaxation) + double LONGITUDINAL_STIFFNESS = 300000; // Longitudinal carcass stiffness + double LATERAL_STIFFNESS = 100000; // Lateral carcass stiffness + double YAW_STIFFNESS = 5000; // Torsional stiffness + double FREQ_LONG = 80; // Longitudinal relaxation freq + double FREQ_LAT = 40; // Lateral relaxation freq + double FREQ_YAW = 50; // Yaw relaxation freq + double FREQ_WINDUP = 70; // Wind-up relaxation freq + double DAMP_LONG = 0.04; // Longitudinal damping + double DAMP_LAT = 0.04; // Lateral damping + double DAMP_YAW = 0.04; // Yaw damping + double DAMP_WINDUP = 0.04; // Wind-up damping + double DAMP_RESIDUAL = 0.002; // Residual damping + double DAMP_VLOW = 0.001; // Low-speed damping + double Q_BVX = 0; // Fx–V coupling + double Q_BVT = 0; // Fy–V coupling + double PCFX1 = 0; // Fx structural shape + double PCFX2 = 0; + double PCFX3 = 0; + double PCFY1 = 0; // Fy structural shape + double PCFY2 = 0; + double PCFY3 = 0; + double PCMZ1 = 0; // Mz structural shape + + // ---------------- contact patch + double Q_RA1 = 0.5; // Patch length scaling + double Q_RA2 = 1; // Patch load effect + double Q_RB1 = 1; // Patch width scaling + double Q_RB2 = -1; // Patch width load effect + double ELLIPS_SHIFT = 0.8; // Ellipse shift + double ELLIPS_LENGTH = 1; // Ellipse length + double ELLIPS_HEIGHT = 1; // Ellipse height + double ELLIPS_ORDER = 1.8; // Ellipse shape order + double ELLIPS_MAX_STEP = 0.025; // Patch discretization + double ELLIPS_NWIDTH = 10; // Patch grid width + double ELLIPS_NLENGTH = 10; // Patch grid length + + // ---------------- ranges / limits + double PRESMIN = 10000; // Min pressure + double PRESMAX = 1000000; // Max pressure + double FZMIN = 228.6; // Min vertical load + double FZMAX = 1600; // Max vertical load + double KPUMIN = -0.25; // Min slip ratio + double KPUMAX = 0.25; // Max slip ratio + double ALPMIN = -0.2094; // Min slip angle + double ALPMAX = 0.2094; // Max slip angle + double CAMMIN = -0.1047; // Min camber + double CAMMAX = 0.1047; // Max camber + + // ---------------- scaling coefficients + double LFZO = 1; // Load scaling + double LCX = 1; // Fx shape scaling + double LMUX = 1; // Longitudinal friction scaling + double LEX = 1; // Fx curvature scaling + double LKX = 1; // Fx stiffness scaling + double LHX = 0; // Fx horizontal shift + double LVX = 0; // Fx vertical shift + double LCY = 1; // Fy shape scaling + double LMUY = 1; // Lateral friction scaling + double LEY = 1; // Fy curvature scaling + double LKY = 1; // Fy stiffness scaling + double LHY = 0; // Fy horizontal shift + double LVY = 0; // Fy vertical shift + double LTR = 1; // Trail scaling + double LRES = 0; // Residual moment scaling + double LXAL = 1; // Alpha scaling + double LYKA = 1; // Combined-slip scaling + double LVYKA = 1; // Combined-slip shift + double LS = 1; // Slip scaling + double LKYC = 1; // Combined Fy stiffness + double LKZC = 1; // Combined Mz stiffness + double LVMX = 0; // Fx moment shift + double LMX = 1; // Fx moment scaling + double LMY = 1; // Fy moment scaling + double LMP = 1; // Pressure scaling + + // ---------------- longitudinal MF (Fx) + double PCX1 = 1.786; // Fx shape factor + double PDX1 = 2.933; // Fx peak friction + double PDX2 = -0.44; // Fx load sensitivity + double PDX3 = 20.8; // Fx camber sensitivity + double PEX1 = 0.871; // Fx curvature + double PEX2 = -0.038; + double PEX3 = 0; + double PEX4 = 0.071; + double PKX1 = 85.31; // Fx stiffness + double PKX2 = -20.25; // Fx stiffness load effect + double PKX3 = 0.5; // Fx stiffness camber effect + double PHX1 = 0; // Fx horizontal shift + double PHX2 = 0; + double PVX1 = 0; // Fx vertical shift + double PVX2 = 0; + double PPX1 = 0; // Fx pressure effect + double PPX2 = 0; + double PPX3 = 0; + double PPX4 = 0; + double RBX1 = 23.72; // Combined-slip factor + double RBX2 = 25.97; + double RBX3 = 0; + double RCX1 = 0.7495; + double REX1 = -0.4759; + double REX2 = 0.8109; + double RHX1 = 0; + + // ---------------- lateral MF (Fy) + double PCY1 = 1.6384; // Fy shape factor + double PDY1 = 2.3749; // Fy peak friction + double PDY2 = -0.1985; // Fy load sensitivity + double PDY3 = 4.0072; // Fy camber sensitivity + double PEY1 = 0.53284; // Fy curvature + double PEY2 = -0.33312; + double PEY3 = 0.27748; + double PEY4 = -0.016493; + double PEY5 = 0; + double PKY1 = -38.9068; // Fy stiffness + double PKY2 = 1.6336; + double PKY3 = 0.62971; + double PKY4 = 2; + double PKY5 = 0; + double PKY6 = -1; + double PKY7 = 0; + double PHY1 = 0; // Fy horizontal shift + double PHY2 = 0; + double PVY1 = 0; // Fy vertical shift + double PVY2 = 0; + double PVY3 = -3.2711; + double PVY4 = -1.1442; + double PPY1 = 0; // Fy pressure effect + double PPY2 = 0; + double PPY3 = 0; + double PPY4 = 0; + double PPY5 = 0; + + // ---------------- rolling / aligning + double QSY1 = -0.0309; // Rolling resistance + double QSY2 = -0.0921; + double QSY7 = 0.85; // Speed effect + double QSY8 = -0.4; + + double QBZ1 = 10; // Aligning stiffness + double QCZ1 = 1.1; // Aligning shape + double QDZ1 = 0.12; // Aligning peak + double QDZ8 = -0.05; // Aligning camber effect + + // ---------------- turnslip + double PECP1 = 1.0; // Camber reduction factors + double PECP2 = 0; + double PDXP1 = 0; // Base reduction of force due to slip + double PDXP2 = 0; + double PDXP3 = 0; + double PDXP4 = 0; + double PDYP1 = 0; + double PDYP2 = 0; + double PDYP3 = 0; + double PDYP4 = 0; + double PKYP1 = 35; + double PHYP1 = 0.2; // base lateral shift + double PHYP2 = 0.1; // load-dependent shift + double PHYP3 = 0.3; // camber sensitivity + double PHYP4 = 0.0; // small smoothing offset +}; + +struct VehicleModelParams { + TireParameters tire_parameters; + float camber_scaling_factor; + float effective_tire_r = 1.0; + float distance_to_CG = + 1.0; // MUST BE POSITIVE IF AHEAD OF CG AND NEGATIVE IF BEHIND OTHERWISE PACEJKA WILL BREAK +}; + +struct VehicleModelState { + Velocity velocity; + Force force; + double angular_speed; + double steering_angle = 0.0; + double yaw_rate; +}; + +/* + Struct containing the tire radius, camber... +*/ diff --git a/src/invictasim/src/config/config_loader.cpp b/src/invictasim/src/config/config_loader.cpp index 119147cbb..fe7eec32e 100644 --- a/src/invictasim/src/config/config_loader.cpp +++ b/src/invictasim/src/config/config_loader.cpp @@ -2,7 +2,8 @@ #include -#include "common_lib/config_load/config_load.hpp" +#include + #include "rclcpp/rclcpp.hpp" namespace invictasim { diff --git a/src/invictasim/src/vehicle_model/FSFEUP02_model.cpp b/src/invictasim/src/vehicle_model/FSFEUP02_model.cpp new file mode 100644 index 000000000..4591e3a52 --- /dev/null +++ b/src/invictasim/src/vehicle_model/FSFEUP02_model.cpp @@ -0,0 +1,94 @@ +#include "vehicle_model/FSFEUP02_model.hpp" + +FSFEUP02Model::FSFEUP02Model(const std::string& config_path) { + YAML::Node config = YAML::LoadFile(config_path); + + auto model_config = config["vehicle_model"]["FSFEUP02_model"]; + + // Load kinematics parameters + // lr_ = model_config["kinematics"]["lr"].as(); + // lf_ = model_config["kinematics"]["lf"].as(); + // sf_ = model_config["kinematics"]["sf"].as(); + // sr_ = model_config["kinematics"]["sr"].as(); + // h_cg_ = model_config["kinematics"]["h_cg"].as(); + // max_steering_angle_ = + // model_config["kinematics"]["max_steering_angle"].as() * M_PI / 180.0; + + // IMPORTANT: LOAD THE TIRE CONFIGURATION BASED ON THE TIRE FIELD + + // Load tire parameters + // float effective_wheel_radius = model_config["tire"]["effective_tire_radius"].as(); + float d_bleft = model_config["tire"]["d_bleft"].as(); + float d_bright = model_config["tire"]["d_bright"].as(); + float d_fleft = model_config["tire"]["d_fleft"].as(); + float d_fright = model_config["tire"]["d_fright"].as(); + float camber = model_config["tire"]["camber"].as(); + + // Initialize tires + front_left = std::make_unique(camber, d_fleft); + front_right = std::make_unique(camber, d_fright); + back_left = std::make_unique(camber, d_bleft); + back_right = std::make_unique(camber, d_bright); + + // Load aerodynamics + // cla_ = model_config["aero"]["cla"].as(); + // cda_ = model_config["aero"]["cda"].as(); + // aero_area_ = model_config["aero"]["aeroArea"].as(); + + // Load mass and inertia + mass_ = model_config["m"].as(); + Izz_ = model_config["Izz"].as(); + + // Load drivetrain parameters + wheel_radius_ = model_config["wheelRadius"].as(); + gear_ratio_ = model_config["gearRatio"].as(); + + // Initialize state variables + x_ = 0.0; + y_ = 0.0; + yaw_ = 0.0; + vx_ = 0.0; + steering_angle_ = 0.0; + throttle_ = 0.0; +} + +void FSFEUP02Model::step(double dt) { + (void)dt; + // Some Logic, Bicycle model class would have a MotorModel, TireModel, etc and it will + // call them here or something similar +} + +void FSFEUP02Model::reset() { + x_ = 0.0; + y_ = 0.0; + yaw_ = 0.0; + vx_ = 0.0; + steering_angle_ = 0.0; + throttle_ = 0.0; +} + +void FSFEUP02Model::set_position(double x, double y, double yaw) { + x_ = x; + y_ = y; + yaw_ = yaw; +} + +void FSFEUP02Model::set_velocity(double vx) { vx_ = vx; } + +double FSFEUP02Model::get_position_x() const { return x_; } + +double FSFEUP02Model::get_position_y() const { return y_; } + +double FSFEUP02Model::get_yaw() const { return yaw_; } + +double FSFEUP02Model::get_velocity_x() const { return vx_; } + +void FSFEUP02Model::set_steering(double angle) { steering_angle_ = angle; } + +void FSFEUP02Model::set_throttle(double throttle) { throttle_ = throttle; } + +std::string FSFEUP02Model::get_model_name() const { return "FSFEUP02Model"; } + +float FSFEUP02Model::get_tire_effective_radius() const { + return front_left->get_tire_effective_radius(); +} diff --git a/src/invictasim/src/vehicle_model/tire_model/tire_model.cpp b/src/invictasim/src/vehicle_model/tire_model/tire_model.cpp new file mode 100644 index 000000000..da1cefbd9 --- /dev/null +++ b/src/invictasim/src/vehicle_model/tire_model/tire_model.cpp @@ -0,0 +1,369 @@ +#include "vehicle_model/tire_model/tire_model.hpp" + +#include + +// CAUTION: AUXILIARY FUNCTION HERE FOR NOW MAYBE MOVE IT LATER +int sign(float val) { return (val > 0) ? 1 : -1; } + +TireModel::TireModel(float camber, float dist) { + InternalValues internal_vals = {}; + internal_vals.camber_angle = camber; + internal_vals.distance_to_CG = dist; +} + +float TireModel::calculateSlipAngle() const { + double v_x = vehicle_model_state.velocity.vx; + if (v_x == 0.0) v_x = 0.1; + float numerator = vehicle_model_state.velocity.vy + + (internal_vals.distance_to_CG * vehicle_model_state.yaw_rate); + return atan2( + numerator, + v_x); // atan2 permite cobrir todos os ranges de angulo e não apenas de -pi/2 até pi/2 +} + +float TireModel::calculateSlipRatio() const { + double v_x = vehicle_model_state.velocity.vx; + if (v_x == 0.0) v_x = 0.1; + float numerator = + (vehicle_model_params.effective_tire_r * vehicle_model_state.angular_speed) - v_x; + return numerator / v_x; +} + +TireOutput TireModel::calculateForces() const { + float slip_a = calculateSlipAngle(); + float slip_r = calculateSlipRatio(); + float Dy = calculateDy(); + float Cy = calculateCy(); + float By = calculateBy(Dy, Cy); + float Ey = calculateEy(slip_a); + float Dx = calculateDx(); + float Cx = calculateCx(); + float Bx = calculateBx(Dx, Cx); + float Ex = calculateEx(slip_r); + float SVx = calculateSVx(); + float SVy = calculateSVy(); + TireOutput out{}; + out.Fx = calculatePacejka(Bx, Cx, Dx, Ex, slip_r, SVx); + out.Fy = calculatePacejka(By, Cy, Dy, Ey, slip_a, SVy); + return out; +} + +float TireModel::calculatePacejka(float B, float C, float D, float E, float slip, float SV) const { + return D * sin(C * atan(B * slip - E * (B * slip - atan(B * slip)))) + SV; +} + +/* +--------------------------- + D calculation: + muy = (PDY1 + PDY2 .* dfz).*(1 + PPY3.*dpi + PPY4 .*dpi.^2).*(1 +-PDY3.*gamma_star.^2).*LMUY_star; % (4.E23) + Dy = muy.*Fz.*zeta2; % (4.E22) +------------------------------ +*/ + +bool TireModel::calculateTireState(float slip_angle, float slip_ratio) { + internal_vals.Fz0_prime = + vehicle_model_params.tire_parameters.LFZO * vehicle_model_state.force.fz; + internal_vals.dfz = + (vehicle_model_state.force.fz - internal_vals.Fz0_prime) / internal_vals.Fz0_prime; + internal_vals.epsilong = vehicle_model_params.tire_parameters.PECP1 * + (1 + vehicle_model_params.tire_parameters.PECP2 * internal_vals.dfz); + internal_vals.Vc_prime = sqrt(vehicle_model_state.velocity.vx * vehicle_model_state.velocity.vx + + vehicle_model_state.velocity.vy * vehicle_model_state.velocity.vy); + internal_vals.phi = (1 / internal_vals.Vc_prime) * + (vehicle_model_state.yaw_rate - (1 - internal_vals.epsilong) * + vehicle_model_state.angular_speed * + sin(internal_vals.camber_angle)); + internal_vals.gamma_star = + internal_vals.camber_angle * vehicle_model_params.camber_scaling_factor; + double Byp = vehicle_model_params.tire_parameters.PDYP1 * + (1 + vehicle_model_params.tire_parameters.PDYP2 * internal_vals.dfz) * + cos(atan(vehicle_model_params.tire_parameters.PDYP3 * tan(slip_angle))); + internal_vals.zeta2 = cos(atan( + Byp * + (vehicle_model_params.tire_parameters.UNLOADED_RADIUS * abs(internal_vals.phi) + + vehicle_model_params.tire_parameters.PDYP4 * + sqrt(vehicle_model_params.tire_parameters.UNLOADED_RADIUS * abs(internal_vals.phi))))); + internal_vals.zeta3 = + cos(atan(vehicle_model_params.tire_parameters.PKYP1 * + (vehicle_model_params.effective_tire_r * vehicle_model_params.effective_tire_r) * + (internal_vals.phi * internal_vals.phi))); + internal_vals.Kya = + vehicle_model_params.tire_parameters.PKY1 * internal_vals.Fz0_prime * + (1 + vehicle_model_params.tire_parameters.PPY1 * internal_vals.dpi) * + (1 - vehicle_model_params.tire_parameters.PKY3 * abs(internal_vals.gamma_star)) * + sin(vehicle_model_params.tire_parameters.PKY4 * + atan((vehicle_model_state.force.fz / internal_vals.Fz0_prime) / + ((vehicle_model_params.tire_parameters.PKY2 + + vehicle_model_params.tire_parameters.PKY5 * internal_vals.gamma_star * + internal_vals.gamma_star) * + (1 + vehicle_model_params.tire_parameters.PPY2 * internal_vals.dpi)))) * + internal_vals.zeta3 * vehicle_model_params.tire_parameters.LKY; + internal_vals.Vcx = (vehicle_model_state.velocity.vx * cos(vehicle_model_state.steering_angle)) + + (vehicle_model_state.velocity.vy + + vehicle_model_state.yaw_rate * internal_vals.distance_to_CG) * + sin(vehicle_model_state.steering_angle); + double Bxp = vehicle_model_params.tire_parameters.PDXP1 * + (1 + vehicle_model_params.tire_parameters.PDXP2 * internal_vals.dfz) * + cos(atan(vehicle_model_params.tire_parameters.PDXP3 * slip_ratio)); + internal_vals.zeta1 = cos(atan(Bxp * vehicle_model_params.effective_tire_r * internal_vals.phi)); + internal_vals.LMUY_prime = vehicle_model_params.tire_parameters.Amu * + vehicle_model_params.tire_parameters.LMUY / + (1 + (vehicle_model_params.tire_parameters.Amu - 1) * + vehicle_model_params.tire_parameters.LMUY); + internal_vals.SVyg = vehicle_model_state.force.fz * + (vehicle_model_params.tire_parameters.PVY3 + + vehicle_model_params.tire_parameters.PVY4 * internal_vals.dfz) * + internal_vals.gamma_star * vehicle_model_params.tire_parameters.LKYC * + internal_vals.LMUY_prime * internal_vals.zeta2; + return true; +} + +float TireModel::calculateDy() const { + double muy = + (vehicle_model_params.tire_parameters.PDY1 + + vehicle_model_params.tire_parameters.PDY2 * internal_vals.dfz) * + (1 + vehicle_model_params.tire_parameters.PPY3 * internal_vals.dpi + + vehicle_model_params.tire_parameters.PPY4 * (internal_vals.dpi * internal_vals.dpi)) * + (1 - vehicle_model_params.tire_parameters.PDY3 * + (internal_vals.gamma_star * internal_vals.gamma_star)) * + vehicle_model_params.tire_parameters + .LMUY; // we can assume dpi is zero for now and ignore pressure variations + + return muy * vehicle_model_state.force.fz * internal_vals.zeta2; +} + +float TireModel::calculateDx() const { + if (vehicle_model_state.force.fz > 0) { + double mux = + (vehicle_model_params.tire_parameters.PDX1 + + vehicle_model_params.tire_parameters.PDX2 * internal_vals.dfz) * + (1 + vehicle_model_params.tire_parameters.PPX3 * internal_vals.dpi + + vehicle_model_params.tire_parameters.PPX4 * (internal_vals.dpi * internal_vals.dpi)) * + (1 - vehicle_model_params.tire_parameters.PDX3 * + (internal_vals.camber_angle * internal_vals.camber_angle)) * + vehicle_model_params.tire_parameters.LMUX; + return mux * vehicle_model_state.force.fz * internal_vals.zeta1; + } + return 0; +} + +float TireModel::calculateCx() const { + if (vehicle_model_state.force.fz > 0) { + return vehicle_model_params.tire_parameters.PCX1 * vehicle_model_params.tire_parameters.LCX; + } + return 0; +} + +float TireModel::calculateCy() const { + if (vehicle_model_state.force.fz > 0) { + return vehicle_model_params.tire_parameters.PCY1 * vehicle_model_params.tire_parameters.LCY; + } + return 0; +} + +float TireModel::calculateBx(float Dx, float Cx) const { + double Kxk = + vehicle_model_state.force.fz * + (vehicle_model_params.tire_parameters.PKX1 + + vehicle_model_params.tire_parameters.PKX2 * internal_vals.dfz) * + exp(vehicle_model_params.tire_parameters.PKX3 * internal_vals.dfz) * + (1 + vehicle_model_params.tire_parameters.PPX1 * internal_vals.dpi + + vehicle_model_params.tire_parameters.PPX2 * (internal_vals.dpi * internal_vals.dpi)) * + vehicle_model_params.tire_parameters.LKX; + return Kxk / (Cx * Dx + internal_vals.epsilon * sign(Dx)); +} + +float TireModel::calculateBy(float Dy, float Cy) const { + return internal_vals.Kya / (Cy * Dy + internal_vals.epsilon * sign(Dy)); +} + +float TireModel::calculateEx(double slip_ratio) const { + double SHx = (vehicle_model_params.tire_parameters.PHX1 + + vehicle_model_params.tire_parameters.PHX2 * internal_vals.dfz) * + vehicle_model_params.tire_parameters.LHX; + double kappax = slip_ratio + SHx; + double Ex = + (vehicle_model_params.tire_parameters.PEX1 + + vehicle_model_params.tire_parameters.PEX2 * internal_vals.dfz + + vehicle_model_params.tire_parameters.PEX3 * (internal_vals.dfz * internal_vals.dfz)) * + (1 - vehicle_model_params.tire_parameters.PEX4 * sign(kappax)) * + vehicle_model_params.tire_parameters.LEX; + return (Ex < 1) ? Ex : 1.0; // E cannot be over 1 +} + +float TireModel::calculateEy(double slip_angle) const { + // MF 6.1 and 6.2 + double Kyg0 = vehicle_model_state.force.fz * + (vehicle_model_params.tire_parameters.PKY6 + + vehicle_model_params.tire_parameters.PKY7 * internal_vals.dfz) * + (1 + vehicle_model_params.tire_parameters.PPY5 * internal_vals.dpi) * + vehicle_model_params.tire_parameters.LKYC; + double DHyp = (vehicle_model_params.tire_parameters.PHYP2 + + vehicle_model_params.tire_parameters.PHYP3 * internal_vals.dfz) * + sign(internal_vals.Vcx); + double KyRp0 = Kyg0 / (1 - internal_vals.epsilong); + double Kya0 = vehicle_model_params.tire_parameters.PKY1 * internal_vals.Fz0_prime * + (1 + vehicle_model_params.tire_parameters.PPY1 * internal_vals.dpi) * + sin(vehicle_model_params.tire_parameters.PKY4 * + atan((vehicle_model_state.force.fz / internal_vals.Fz0_prime) / + (vehicle_model_params.tire_parameters.PKY2 * + (1 + vehicle_model_params.tire_parameters.PPY2 * internal_vals.dpi)))) * + internal_vals.zeta3 * vehicle_model_params.tire_parameters.LKY; + double Kyao_prime = Kya0 + internal_vals.epsilon * sign(Kya0); + double BHyp = KyRp0 / (vehicle_model_params.tire_parameters.PHYP1 * DHyp * Kyao_prime); + double EHyp = (vehicle_model_params.tire_parameters.PHYP4 > 1) + ? 1 + : vehicle_model_params.tire_parameters.PHYP4; + double SHyp = + DHyp * + sin(vehicle_model_params.tire_parameters.PHYP1 * + atan(BHyp * vehicle_model_params.effective_tire_r * internal_vals.phi - + EHyp * (BHyp * vehicle_model_params.effective_tire_r * internal_vals.phi - + atan(BHyp * vehicle_model_params.effective_tire_r * internal_vals.phi)))) * + sign(internal_vals.Vcx); + double Kya_prime = internal_vals.Kya + internal_vals.epsilon * sign(internal_vals.Kya); + double zeta4 = 1 + SHyp - internal_vals.SVyg / Kya_prime; + double SHy = (vehicle_model_params.tire_parameters.PHY1 + + vehicle_model_params.tire_parameters.PHY2 * internal_vals.dfz) * + vehicle_model_params.tire_parameters.LHY + + ((Kyg0 * internal_vals.gamma_star - internal_vals.SVyg) / + (internal_vals.Kya + internal_vals.epsilon * sign(internal_vals.Kya))) + + zeta4 - 1; + double alpha_y = tan(slip_angle) + SHy; + double Ey = (vehicle_model_params.tire_parameters.PEY1 + + vehicle_model_params.tire_parameters.PEY2 * internal_vals.dfz) * + (1 + + vehicle_model_params.tire_parameters.PEY5 * + (internal_vals.gamma_star * internal_vals.gamma_star) - + (vehicle_model_params.tire_parameters.PEY3 + + vehicle_model_params.tire_parameters.PEY4 * internal_vals.gamma_star) * + sign(alpha_y)) * + vehicle_model_params.tire_parameters.LEY; + return (Ey < 1) ? Ey : 1.0; // E cannot be over 1 +} + +float TireModel::calculateSVx() const { + double LMUX_prime = vehicle_model_params.tire_parameters.Amu * + vehicle_model_params.tire_parameters.LMUX / + (1 + (vehicle_model_params.tire_parameters.Amu - 1) * + vehicle_model_params.tire_parameters.LMUX); + return vehicle_model_state.force.fz * + (vehicle_model_params.tire_parameters.PVX1 + + vehicle_model_params.tire_parameters.PVX2 * internal_vals.dfz) * + vehicle_model_params.tire_parameters.LVX * LMUX_prime * internal_vals.zeta1; +} + +float TireModel::calculateSVy() const { + return vehicle_model_state.force.fz * + (vehicle_model_params.tire_parameters.PVY1 + + vehicle_model_params.tire_parameters.PVY2 * internal_vals.dfz) * + vehicle_model_params.tire_parameters.LVY * internal_vals.LMUY_prime * + internal_vals.zeta2 + + internal_vals.SVyg; +} + +float TireModel::get_tire_effective_radius() const { + return vehicle_model_params.effective_tire_r; +}; + +/* +------------------------------------------------ + VD Implementation +------------------------------------------------ +*/ + +/* + Relevant parameters: + - Q_FZ2 - Quadratic term in load vs defelction + - epsilong -> camber reduction factor + - gamma -> camberAngle + - LMUX -> Scale factor of Fx peak friction coeficient + - LMUY -> Scale factor of Fx peak friction coeficient + - PDY1,2,3 + - PPY3,4 +*/ + +/* +Relevant equations for Fx and Fy: + + ------------------------------------------------------------------- + D calculation: + + ux = (PDX1 + PDX2.*dfz).*(1 + PPX3.*dpi + PPX4.*dpi.^2).*(1 -PDX3.*gamma.^2).*LMUX_star; +% (4.E13) mux(Fz==0) = 0; % Zero Fz correction Dx = mux.*Fz.*zeta1; % (> 0) (4.E12) + ----------------------------------------------------------------------- + + + ----------------------------------------------------------------------- + C calculation: + Cx = PCX1.*LCX; % (> 0) (4.E11) + -------------------------------------------------------------------------- + + ---------------------------------------------------------------------------- + B calculation: + Kxk = Fz.*(PKX1 + PKX2.*dfz).*exp(PKX3.*dfz).*(1 + PPX1.*dpi + +PPX2.*dpi.^2).*LKX; % (= BxCxDx = dFxo./dkx at kappax = 0) (= Cfk) (4.E15) + + signDx = sign(Dx); + signDx(signDx == 0) = 1; % If [Dx = 0] then [sign(0) = 0]. This is done to avoid [Kxk / +0 = NaN] in Eqn 4.E16 + + Bx = Kxk./(Cx.*Dx + epsilonx.*signDx); % (4.E16) [sign(Dx) term explained on page 177] + --------------------------------------------------------------------------- + + + ----------------------------------------------- + E calculation: + Ex = (PEX1 + PEX2.*dfz + PEX3.*dfz.^2).*(1 - PEX4.*sign(kappax)).*LEX; % (<=1) (4.E14) + ------------------------------------------------ + + --------------------------------- + Fx: + Fx0 = Dx.*sin(Cx.*atan(Bx.*kappax-Ex.*(Bx.*kappax-atan(Bx.*kappax))))+SVx; % +(4.E9) + ----------------------------------- +*/ + +/* + Relevant equations for Fy: + + -------------------------- + C calculation: + Cy = PCY1.*LCY; % (> 0) (4.E21) + -------------------------- + + --------------------------- + D calculation: + muy = (PDY1 + PDY2 .* dfz).*(1 + PPY3.*dpi + PPY4 .*dpi.^2).*(1 - + PDY3.*gamma_star.^2).*LMUY_star; % (4.E23) Dy = muy.*Fz.*zeta2; % (4.E22) + ------------------------------ + + ------------------------------ + E calculation: + signAlphaY = sign(alphay); + signAlphaY(signAlphaY == 0) = 1; + Ey = (PEY1 + PEY2.*dfz).*(1 + PEY5.*gamma_star.^2 - (PEY3 + + PEY4.*gamma_star).*signAlphaY).*LEY; % (<=1)(4.E24) + ----------------------------- + + ----------------------------- + B calculation: + signDy = sign(Dy); + signDy(signDy == 0) = 1; % If [Dy = 0] then [sign(0) = 0]. This is done to avoid [Kya / + 0 = NaN] in Eqn 4.E26 + + By = Kya./(Cy.*Dy + epsilony.*signDy); % (4.E26) [sign(Dy) term explained on page 177] + ------------------------------ + + + --------------------- + Fy: + Fy0 = Dy .* sin(Cy.*atan(By.*alphay-Ey.*(By.*alphay - atan(By.*alphay))))+ SVy; + % (4.E19) + + + + + +*/ \ No newline at end of file diff --git a/src/invictasim/test/test.cpp b/src/invictasim/test/test.cpp index b9ec21118..3b38ed238 100644 --- a/src/invictasim/test/test.cpp +++ b/src/invictasim/test/test.cpp @@ -1,5 +1,7 @@ -#include "gtest/gtest.h" +#include +#include "../include/vehicle_model/FSFEUP02_model.hpp" +#include "gtest/gtest.h" /** * @brief Example test */ @@ -7,3 +9,15 @@ TEST(invictasim, example_test) { // Just to see if tests are running EXPECT_EQ(1, 1); } + +TEST(FSFEUP02Model_, initializeVehicleModel) { + std::string config_path = "/home/ws/config/invictasim/vehicle_models/FSFEUP02_model.yaml"; + FSFEUP02Model vehicle(config_path); + EXPECT_EQ(vehicle.get_model_name(), "FSFEUP02Model"); +} + +TEST(TireModel_, initializeTireModel) { + std::string config_path = "/home/ws/config/invictasim/vehicle_models/FSFEUP02_model.yaml"; + FSFEUP02Model vehicle(config_path); + EXPECT_EQ(vehicle.get_tire_effective_radius(), 10); +}