404 Complete Boat State Class

.vscode/settings.json

@@ -0,0 +1,22 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/workspaces/sailbot_workspace/build/local_pathfinding",
+        "/workspaces/sailbot_workspace/build/integration_tests",
+        "/workspaces/sailbot_workspace/build/controller",
+        "/workspaces/sailbot_workspace/build/boat_simulator",
+        "/workspaces/sailbot_workspace/install/local/lib/python3.10/dist-packages",
+        "/workspaces/sailbot_workspace/install/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/workspaces/sailbot_workspace/build/local_pathfinding",
+        "/workspaces/sailbot_workspace/build/integration_tests",
+        "/workspaces/sailbot_workspace/build/controller",
+        "/workspaces/sailbot_workspace/build/boat_simulator",
+        "/workspaces/sailbot_workspace/install/local/lib/python3.10/dist-packages",
+        "/workspaces/sailbot_workspace/install/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages"
+    ]
+}

src/boat_simulator/boat_simulator/common/constants.py

@@ -3,9 +3,7 @@
 import os
 from dataclasses import dataclass
 from enum import Enum
-from typing import Dict
 
-import numpy as np
 from numpy.typing import NDArray
 
 from boat_simulator.common.types import Scalar
@@ -38,16 +36,19 @@ class PhysicsEnginePublisherTopics:
 
 @dataclass
 class BoatProperties:
-    sail_lift_coeffs: Dict[Scalar, Scalar]  # Degrees, Dimensionless
-    sail_drag_coeffs: Dict[Scalar, Scalar]  # Degrees, Dimensionless
-    sail_areas: Dict[Scalar, Scalar]  # Degrees, Square meters (m^2)
-    rudder_drag_coeffs: Dict[Scalar, Scalar]  # Degrees, Dimensionless
-    rudder_areas: Dict[Scalar, Scalar]  # Degrees, Square meters (m^2)
+    sail_lift_coeffs: NDArray  # Degrees, Dimensionless
+    sail_drag_coeffs: NDArray  # Degrees, Dimensionless
+    sail_areas: Scalar  # Degrees, Square meters (m^2)
+    rudder_lift_coeffs: NDArray  # Degrees, Dimensionless
+    rudder_drag_coeffs: NDArray  # Degrees, Dimensionless
+    rudder_areas: Scalar  # Degrees, Square meters (m^2)
     sail_dist: Scalar  # Meters (m)
     rudder_dist: Scalar  # Meters (m)
     hull_drag_factor: Scalar  # Dimensionless
     mass: Scalar  # Kilograms (kg)
     inertia: NDArray  # Kilograms-meters squared (kg•m^2)
+    air_density: Scalar  # Kilograms per meter cubed (kg/m^3)
+    water_density: Scalar  # Kilograms per meter cubed (kg/m^3)
 
 
 # Directly accessible constants
@@ -88,17 +89,15 @@ class BoatProperties:
 # Max sail actuator control angle range in degrees, min angle [0], max angle [1]
 SAIL_MAX_ANGLE_RANGE = (-7, 7)
 
-# Predetermined values for BoatProperties
-# TODO These are placeholder values which should be replaced when we have real values.
-BOAT_PROPERTIES = BoatProperties(
-    sail_lift_coeffs={0.0: 0.0, 5.0: 0.2, 10.0: 0.5, 15.0: 0.7, 20.0: 1.0},
-    sail_drag_coeffs={0.0: 0.1, 5.0: 0.12, 10.0: 0.15, 15.0: 0.18, 20.0: 0.2},
-    sail_areas={0.0: 20.0, 5.0: 19.8, 10.0: 19.5, 15.0: 19.2, 20.0: 18.8},
-    rudder_drag_coeffs={0.0: 0.2, 5.0: 0.22, 10.0: 0.25, 15.0: 0.28, 20.0: 0.3},
-    rudder_areas={0.0: 2.0, 5.0: 1.9, 10.0: 1.8, 15.0: 1.7, 20.0: 1.6},
-    sail_dist=5.0,
-    rudder_dist=1.5,
-    hull_drag_factor=0.05,
-    mass=200.0,
-    inertia=np.array([[10, 0, 0], [0, 30, 0], [0, 0, 20]], dtype=np.float32),
-)
+# # Predetermined values for BoatProperties
+# # TODO These are placeholder values which should be replaced when we have real values.
+# BOAT_PROPERTIES = BoatProperties(
+#     sail_lift_coeffs={0.0: 0.0, 5.0: 0.2, 10.0: 0.5, 15.0: 0.7, 20.0: 1.0},
+#     sail_drag_coeffs={0.0: 0.1, 5.0: 0.12, 10.0: 0.15, 15.0: 0.18, 20.0: 0.2},
+#     sail_areas={0.0: 20.0, 5.0: 19.8, 10.0: 19.5, 15.0: 19.2, 20.0: 18.8},
+#     rudder_drag_coeffs={0.0: 0.2, 5.0: 0.22, 10.0: 0.25, 15.0: 0.28, 20.0: 0.3},
+#     rudder_areas={0.0: 2.0, 5.0: 1.9, 10.0: 1.8, 15.0: 1.7, 20.0: 1.6},
+#     sail_dist=5.0,
+#     rudder_dist=1.5,
+#     hull_drag_factor=0.05,
+# )

src/boat_simulator/boat_simulator/nodes/physics_engine/model.py

@@ -7,6 +7,7 @@
 
 from boat_simulator.common.constants import BoatProperties
 from boat_simulator.common.types import Scalar
+from boat_simulator.nodes.physics_engine.fluid_forces import MediumForceComputation
 from boat_simulator.nodes.physics_engine.kinematics_computation import BoatKinematics
 from boat_simulator.nodes.physics_engine.kinematics_data import KinematicsData
 
@@ -31,6 +32,19 @@ def __init__(self, timestep: Scalar):
             timestep, BoatProperties.mass, BoatProperties.inertia
         )
 
+        self.__sail_force_computation = MediumForceComputation(
+            BoatProperties.sail_lift_coeffs,
+            BoatProperties.sail_drag_coeffs,
+            BoatProperties.sail_areas,
+            BoatProperties.air_density,
+        )
+        self.__rudder_force_computation = MediumForceComputation(
+            BoatProperties.rudder_lift_coeffs,
+            BoatProperties.rudder_drag_coeffs,
+            BoatProperties.rudder_areas,
+            BoatProperties.water_density,
+        )
+
     def step(
         self,
         glo_wind_vel: NDArray,
@@ -88,7 +102,40 @@ def __compute_net_force_and_torque(
                 the relative reference frame, expressed in newtons (N), and the second element
                 represents the net torque, expressed in newton-meters (N•m).
         """
-        raise NotImplementedError()
+        # Compute apparent wind and water velocities as ND arrays
+        apparent_wind_vel = np.subtract(rel_wind_vel, self.relative_velocity)
+        apparent_water_vel = np.subtract(rel_water_vel, self.relative_velocity)
+
+        # Calculate Forces on sail and rudder
+        sail_force = self.__sail_force_computation.compute(apparent_wind_vel, trim_tab_angle)
+        rudder_force = self.__rudder_force_computation.compute(
+            apparent_water_vel, rudder_angle_deg
+        )
+
+        # Calculate Hull Drag Force
+        hull_drag_force = (
+            self.relative_velocity[0] * BoatProperties.hull_drag_factor,
+            self.relative_velocity[1] * BoatProperties.hull_drag_factor,
+        )
+
+        # Total Force Calculation
+        total_drag_force = np.add(sail_force[1] + rudder_force[1] + hull_drag_force)
+        total_force = np.add(sail_force[0] + rudder_force[0], total_drag_force)
+
+        # Calculating Total Torque
+        sail_torque_constant = BoatProperties.sail_dist * np.sin(np.radians(trim_tab_angle))
+        rudder_torque_constant = BoatProperties.rudder_dist * np.sin(np.radians(rudder_angle_deg))
+
+        sail_torque = (sail_force[0] * sail_torque_constant, sail_force[1] * sail_torque_constant)
+        rudder_torque = (
+            rudder_force[0] * rudder_torque_constant,
+            rudder_force[1] * rudder_torque_constant,
+        )
+        total_torque = (sail_torque[0] + rudder_torque[0], sail_torque[1] + rudder_torque[1])
+
+        # torque_magnitude = np.
+
+        return (total_force, total_torque)
 
     @property
     def global_position(self) -> NDArray: