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,7 +3,6 @@
 import os
 from dataclasses import dataclass
 from enum import Enum
-from typing import Dict
 
 import numpy as np
 from numpy.typing import NDArray
@@ -38,21 +37,22 @@ class PhysicsEnginePublisherTopics:
 
 @dataclass
 class BoatProperties:
-    # A lookup table that maps angles of attack (in degrees) to their corresponding lift
+    # A lookup array that maps angles of attack (in degrees) to their corresponding lift
     # coefficients.
-    sail_lift_coeffs: Dict[Scalar, Scalar]
-    # A lookup table that maps angles of attack (in degrees) to their corresponding drag
+    sail_lift_coeffs: NDArray
+    # A lookup array that maps angles of attack (in degrees) to their corresponding drag
     # coefficients.
-    sail_drag_coeffs: Dict[Scalar, Scalar]
-    # A lookup table that maps angles of attack (in degrees) to their corresponding sail areas
-    # (in square meters).
-    sail_areas: Dict[Scalar, Scalar]
-    # A lookup table that maps angles of attack (in degrees) to their corresponding drag
+    sail_drag_coeffs: NDArray
+    # The area of sail (in square meters).
+    sail_areas: Scalar
+    # A lookup array that maps angles of attack (in degrees) to their corresponding lift
     # coefficients for the rudder.
-    rudder_drag_coeffs: Dict[Scalar, Scalar]
-    # A lookup table that maps angles of attack (in degrees) to their corresponding rudder areas
-    # (in square meters).
-    rudder_areas: Dict[Scalar, Scalar]
+    rudder_lift_coeffs: NDArray
+    # A lookup array that maps angles of attack (in degrees) to their corresponding drag
+    # coefficients for the rudder.
+    rudder_drag_coeffs: NDArray
+    # The area of rudder (in square meters).
+    rudder_areas: Scalar
     # A scalar representing the distance from the center of effort of the sail to the pivot point
     # (in meters).
     sail_dist: Scalar
@@ -66,6 +66,15 @@ class BoatProperties:
     mass: Scalar
     # The inertia of the boat (in kilograms-meters squared).
     inertia: NDArray
+    # The density of air (in kilograms per meter cubed).
+    air_density: Scalar
+    # The density of water (in kilograms per meter cubed).
+    water_density: Scalar
+    # The center of gravity of the boat ((3, ) array in meters).
+    # (0, 0) at bottom right corner
+    centre_of_gravity: NDArray
+    # The mast position ((3, ) array in meters).
+    mast_position: NDArray
 
 
 # Directly accessible constants
@@ -109,14 +118,21 @@ class BoatProperties:
 # Constants related to the physical and mechanical properties of Polaris
 # 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=0.5,
-    rudder_dist=1.0,
+    sail_lift_coeffs=np.array([[0.0, 0.0], [5.0, 0.2], [10.0, 0.5], [15.0, 0.7], [20.0, 1.0]]),
+    sail_drag_coeffs=np.array([[0.0, 0.1], [5.0, 0.12], [10.0, 0.15], [15.0, 0.18], [20.0, 0.2]]),
+    sail_areas=4.0,
+    rudder_lift_coeffs=np.array([[0.0, 0.0], [5.0, 0.1], [10.0, 0.2], [15.0, 0.3], [20.0, 0.4]]),
+    rudder_drag_coeffs=np.array(
+        [[0.0, 0.2], [5.0, 0.22], [10.0, 0.25], [15.0, 0.28], [20.0, 0.3]]
+    ),
+    rudder_areas=3.0,
+    sail_dist=0.75,  # defined distance from mast position to centre of gravity of the sailboat
+    rudder_dist=1.5,
     hull_drag_factor=0.05,
-    mass=1500.0,
+    mass=50,
     inertia=np.array([[125, 0, 0], [0, 1125, 0], [0, 0, 500]], dtype=np.float32),
+    air_density=1.225,
+    water_density=1000,
+    centre_of_gravity=np.array([0.8, 1, 0]),
+    mast_position=np.array([0.8, 1.5, 0]),
 )

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

@@ -7,6 +7,7 @@
 
 from boat_simulator.common.constants import BOAT_PROPERTIES
 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, BOAT_PROPERTIES.mass, BOAT_PROPERTIES.inertia
         )
 
+        self.__sail_force_computation = MediumForceComputation(
+            BOAT_PROPERTIES.sail_lift_coeffs,
+            BOAT_PROPERTIES.sail_drag_coeffs,
+            BOAT_PROPERTIES.sail_areas,
+            BOAT_PROPERTIES.air_density,
+        )
+        self.__rudder_force_computation = MediumForceComputation(
+            BOAT_PROPERTIES.rudder_lift_coeffs,
+            BOAT_PROPERTIES.rudder_drag_coeffs,
+            BOAT_PROPERTIES.rudder_areas,
+            BOAT_PROPERTIES.water_density,
+        )
+
     def step(
         self,
         glo_wind_vel: NDArray,
@@ -88,8 +102,71 @@ 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).
         """
-        # TODO Implement this function
-        return (np.array([0, 0, 0]), np.array([0, 0, 0]))
+        # Compute apparent wind and water velocities as ND arrays (2-D)
+
+        assert np.any(rel_wind_vel), "rel_wind_vel cannot be 0"
+        assert np.any(rel_water_vel), "rel_water_vel cannot be 0"
+
+        apparent_wind_vel = np.subtract(rel_wind_vel, self.relative_velocity[0:2])
+        apparent_water_vel = np.subtract(rel_water_vel, self.relative_velocity[0:2])
+
+        # angle references all in radians
+        wind_angle = np.arctan2(rel_wind_vel[1], rel_wind_vel[0])
+        trim_tab_angle_rad = np.radians(trim_tab_angle)
+        main_sail_angle = wind_angle - trim_tab_angle_rad
+        rudder_angle_rad = np.radians(rudder_angle_deg)
+
+        # Calculate Forces on sail and rudder
+        sail_force = self.__sail_force_computation.compute(apparent_wind_vel[0:2], trim_tab_angle)
+        rudder_force = self.__rudder_force_computation.compute(
+            apparent_water_vel[0:2], rudder_angle_deg
+        )
+
+        # Calculate Hull Drag Force
+        hull_drag_force = self.relative_velocity[0:2] * BOAT_PROPERTIES.hull_drag_factor
+
+        # Total Force Calculation
+        total_drag_force = sail_force[1] + rudder_force[1] + hull_drag_force
+        total_force = sail_force[0] + rudder_force[0] + total_drag_force
+
+        # Calculating magnitudes of sail
+        sail_drag = np.linalg.norm(sail_force[1], ord=2)
+        sail_lift = np.linalg.norm(sail_force[0], ord=2)
+
+        # Calculating Total Torque
+        sail_lift_constant = (
+            BOAT_PROPERTIES.mast_position[1]  # position of sail mount
+            - (
+                BOAT_PROPERTIES.sail_dist * np.cos(main_sail_angle)
+            )  # distance of sail with changes to trim tab angle
+            - BOAT_PROPERTIES.centre_of_gravity[1]  # point to take torque around
+        )
+        sail_drag_constant = BOAT_PROPERTIES.sail_dist * np.sin(main_sail_angle)
+
+        sail_torque = np.add(sail_drag * sail_drag_constant, sail_lift * sail_lift_constant)
+
+        # Calculating magnitudes of rudder
+        rudder_drag = np.linalg.norm(rudder_force[1], ord=2)
+        rudder_lift = np.linalg.norm(rudder_force[0], ord=2)
+
+        rudder_drag_constant = BOAT_PROPERTIES.rudder_dist * np.sin(rudder_angle_rad)
+
+        rudder_lift_constant = (
+            BOAT_PROPERTIES.rudder_dist * np.cos(rudder_angle_rad)
+            + BOAT_PROPERTIES.centre_of_gravity[1]
+        )
+
+        # adding total rudder torque
+        rudder_torque = np.add(
+            rudder_lift * rudder_lift_constant,
+            rudder_drag * rudder_drag_constant,
+        )
+
+        total_torque = np.add(sail_torque, rudder_torque)  # Sum torques about z-axis
+
+        final_torque = np.array([0, 0, total_torque])  # generate 3-D array(only z component)
+
+        return (total_force, final_torque)
 
     @property
     def global_position(self) -> NDArray:

src/boat_simulator/tests/unit/nodes/physics_engine/test_model.py

@@ -1,5 +1,87 @@
 """Tests classes and functions in boat_simulator/nodes/physics_engine/model.py"""
 
+import numpy as np
+import pytest
 
-class TestBoatState:
-    pass
+from boat_simulator.common.constants import BOAT_PROPERTIES
+from boat_simulator.nodes.physics_engine.model import BoatState
+
+
+@pytest.mark.parametrize(
+    "rel_wind_vel, rel_water_vel, rudder_angle_deg, trim_tab_angle, timestep",
+    [
+        (np.array([1, 2]), np.array([1, 2]), 45, 45, 1),
+        (np.array([1, 2]), np.array([1, 2]), 45, 45, 1),
+        (np.array([4, 5]), np.array([7, 8]), 30, 60, 2),
+        (np.array([10, 20]), np.array([15, 25]), 90, 120, 1),
+        (np.array([0, 1]), np.array([1, 1]), 0, 90, 3),
+        (np.array([-1, -2]), np.array([-1, -2]), 180, 270, 2),
+        (np.array([3.5, 4.5]), np.array([1.5, 2.5]), 15, 75, 4),
+        (np.array([100, 200]), np.array([300, 200]), 0, 45, 2),
+        # cannot use 0 vector, or will error
+    ],
+)
+def test_compute_net_force_torque(
+    rel_wind_vel,
+    rel_water_vel,
+    rudder_angle_deg,
+    trim_tab_angle,
+    timestep,
+):
+
+    current_state = BoatState(timestep)
+    net_force = current_state._BoatState__compute_net_force_and_torque(
+        rel_wind_vel, rel_water_vel, rudder_angle_deg, trim_tab_angle
+    )
+
+    app_wind_vel = np.subtract(rel_wind_vel, current_state.relative_velocity[0:2])
+    app_water_vel = np.subtract(rel_water_vel, current_state.relative_velocity[0:2])
+
+    wind_angle = np.arctan2(app_wind_vel[1], app_wind_vel[0])
+    trim_tab_angle_rad = np.radians(trim_tab_angle)
+    main_sail_angle = wind_angle - trim_tab_angle_rad
+    rudder_angle_rad = np.radians(rudder_angle_deg)
+
+    test_sail_force = current_state._BoatState__sail_force_computation.compute(
+        app_wind_vel, trim_tab_angle
+    )
+    test_rudder_force = current_state._BoatState__rudder_force_computation.compute(
+        app_water_vel, rudder_angle_deg
+    )
+
+    hull_drag_force = current_state.relative_velocity[0:2] * BOAT_PROPERTIES.hull_drag_factor
+
+    total_drag_force = test_sail_force[1] + test_rudder_force[1] + hull_drag_force
+    total_force = test_sail_force[0] + test_rudder_force[0] + total_drag_force
+
+    sail_drag = np.linalg.norm(test_sail_force[1], ord=2)
+    sail_lift = np.linalg.norm(test_sail_force[0], ord=2)
+
+    sail_lift_constant = (
+        BOAT_PROPERTIES.mast_position[1]
+        - (BOAT_PROPERTIES.sail_dist * np.cos(main_sail_angle))
+        - BOAT_PROPERTIES.centre_of_gravity[1]
+    )
+
+    sail_drag_constant = BOAT_PROPERTIES.sail_dist * np.sin(main_sail_angle)
+
+    sail_torque = np.add(sail_drag * sail_drag_constant, sail_lift * sail_lift_constant)
+
+    rudder_drag = np.linalg.norm(test_rudder_force[1], ord=2)
+    rudder_lift = np.linalg.norm(test_rudder_force[0], ord=2)
+
+    rudder_drag_constant = BOAT_PROPERTIES.rudder_dist * np.sin(rudder_angle_rad)
+
+    rudder_lift_constant = (
+        BOAT_PROPERTIES.rudder_dist * np.cos(rudder_angle_rad)
+        + BOAT_PROPERTIES.centre_of_gravity[1]
+    )
+
+    rudder_torque = np.add(rudder_lift * rudder_lift_constant, rudder_drag * rudder_drag_constant)
+
+    total_torque = np.add(sail_torque, rudder_torque)
+
+    final_torque = np.array([0, 0, total_torque])
+
+    assert np.allclose(total_force, net_force[0], 0.5)  # checking force
+    assert np.allclose(final_torque, net_force[1], 0.5)  # checking torque

src/network_systems/launch/main_launch.py

@@ -221,12 +221,12 @@ def get_remote_transceiver_description(context: LaunchContext) -> Node:
 def get_local_transceiver_description(context: LaunchContext) -> Node:
     """Gets the launch description for the local_transceiver_node.
 
-        Args:
-            context (LaunchContext): The current launch context.
+    Args:
+        context (LaunchContext): The current launch context.
 
-        Returns:
-            Node: The node object that launches the local_transceiver_node.
-        """
+    Returns:
+        Node: The node object that launches the local_transceiver_node.
+    """
     node_name = "local_transceiver_node"
     ros_parameters = [
         global_launch_config,