diff --git a/src/boat_simulator/boat_simulator/common/constants.py b/src/boat_simulator/boat_simulator/common/constants.py index ee72be13e..43cbc1569 100644 --- a/src/boat_simulator/boat_simulator/common/constants.py +++ b/src/boat_simulator/boat_simulator/common/constants.py @@ -37,21 +37,21 @@ 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: NDArray - # A lookup table that maps angles of attack (in degrees) to their corresponding drag + # A lookup array that maps angles of attack (in degrees) to their corresponding drag # coefficients. sail_drag_coeffs: NDArray - # A lookup table that maps angles of attack (in degrees) to their corresponding sail areas - # (in square meters). + # The area of sail (in square meters). sail_areas: Scalar - # A lookup table that maps angles of attack (in degrees) to their corresponding drag + # A lookup array that maps angles of attack (in degrees) to their corresponding lift + # coefficients for the rudder. + rudder_lift_coeffs: NDArray + # A lookup array that maps angles of attack (in degrees) to their corresponding drag # coefficients for the rudder. - rudder_lift_coeffs: NDArray rudder_drag_coeffs: NDArray - # A lookup table that maps angles of attack (in degrees) to their corresponding rudder areas - # (in square meters). + # 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). @@ -66,10 +66,15 @@ class BoatProperties: mass: Scalar # The inertia of the boat (in kilograms-meters squared). inertia: NDArray - air_density: Scalar # Kilograms per meter cubed (kg/m^3) - water_density: Scalar # Kilograms per meter cubed (kg/m^3) - centre_of_gravity: NDArray # Meters (m) - mast_position: NDArray # Meters (m) + # 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 diff --git a/src/boat_simulator/boat_simulator/nodes/physics_engine/model.py b/src/boat_simulator/boat_simulator/nodes/physics_engine/model.py index e36fbe3e7..b55f19e97 100644 --- a/src/boat_simulator/boat_simulator/nodes/physics_engine/model.py +++ b/src/boat_simulator/boat_simulator/nodes/physics_engine/model.py @@ -5,7 +5,6 @@ import numpy as np from numpy.typing import NDArray -from boat_simulator.common.constants import BOAT_PROPERTIES 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 @@ -103,23 +102,24 @@ 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). """ - # 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) + # Compute apparent wind and water velocities as ND arrays (2-D) + 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]) - wind_angle = np.arctan2(rel_wind_vel[1], rel_wind_vel[0]) # Wind angle in radians + # 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 # in radians + 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, trim_tab_angle) + 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, rudder_angle_deg + apparent_water_vel[0:2], rudder_angle_deg ) # Calculate Hull Drag Force - hull_drag_force = self.relative_velocity * BOAT_PROPERTIES.hull_drag_factor + 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 @@ -152,6 +152,7 @@ def __compute_net_force_and_torque( + BOAT_PROPERTIES.centre_of_gravity[1] ) + # adding total rudder torque rudder_torque = np.add( rudder_lift * rudder_lift_constant, rudder_drag * rudder_drag_constant, @@ -159,7 +160,7 @@ def __compute_net_force_and_torque( total_torque = np.add(sail_torque, rudder_torque) # Sum torques about z-axis - final_torque = np.array([0, 0, total_torque]) + final_torque = np.array([0, 0, total_torque]) # generate 3-D array(only z component) return (total_force, final_torque) diff --git a/src/boat_simulator/tests/unit/nodes/physics_engine/test_model.py b/src/boat_simulator/tests/unit/nodes/physics_engine/test_model.py index 6c83e04a3..a8ac120a7 100644 --- a/src/boat_simulator/tests/unit/nodes/physics_engine/test_model.py +++ b/src/boat_simulator/tests/unit/nodes/physics_engine/test_model.py @@ -8,16 +8,17 @@ @pytest.mark.parametrize( - "rel_wind_vel, rel_water_vel, rudder_angle_deg, trim_tab_angle", + "rel_wind_vel, rel_water_vel, rudder_angle_deg, trim_tab_angle, timestep", [ - (np.array([1, 2, 3]), np.array([1, 2, 3]), 45, 45), - (np.array([1, 2, 3]), np.array([1, 2, 3]), 45, 45), - (np.array([4, 5, 6]), np.array([7, 8, 9]), 30, 60), - (np.array([10, 20, 30]), np.array([15, 25, 35]), 90, 120), - (np.array([0, 0, 0]), np.array([1, 1, 1]), 0, 90), - (np.array([-1, -2, -3]), np.array([-1, -2, -3]), 180, 270), - (np.array([3.5, 4.5, 5.5]), np.array([1.5, 2.5, 3.5]), 15, 75), - (np.array([100, 200, 300]), np.array([300, 200, 100]), 0, 45), + (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 cause divison by zero error ], ) def test_compute_net_force_torque( @@ -25,25 +26,30 @@ def test_compute_net_force_torque( rel_water_vel, rudder_angle_deg, trim_tab_angle, + timestep, ): - current_state = BoatState(1.0) - net_force = current_state.step(rel_wind_vel, rel_water_vel, rudder_angle_deg, trim_tab_angle) + 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) - app_water_vel = np.subtract(rel_water_vel, current_state.relative_velocity) + 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.__sail_force_computation.compute(app_wind_vel, trim_tab_angle) - test_rudder_force = current_state.__rudder_force_computation.compute( + 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 * BOAT_PROPERTIES.hull_drag_factor + 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 @@ -77,4 +83,5 @@ def test_compute_net_force_torque( final_torque = np.array([0, 0, total_torque]) - assert np.equal(net_force, (total_force, final_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