diff --git a/src/boat_simulator/boat_simulator/common/constants.py b/src/boat_simulator/boat_simulator/common/constants.py index 57450b92d..9373e6835 100644 --- a/src/boat_simulator/boat_simulator/common/constants.py +++ b/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 @@ -112,14 +121,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=1500, 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]), ) 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 82905deb8..33f9e3ed3 100644 --- a/src/boat_simulator/boat_simulator/nodes/physics_engine/model.py +++ b/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, @@ -56,12 +70,16 @@ def step( kinematic data in the relative reference frame, and the second element representing data in the global reference frame, both using SI units. """ - rel_wind_vel = glo_wind_vel - self.global_velocity - rel_water_vel = glo_water_vel - self.global_velocity + # Wind and water are 2D (horizontal); use only x,y from global velocity + rel_wind_vel = glo_wind_vel - self.global_velocity[:2] + rel_water_vel = glo_water_vel - self.global_velocity[:2] rel_net_force, net_torque = self.__compute_net_force_and_torque( rel_wind_vel, rel_water_vel, rudder_angle_deg, trim_tab_angle ) + + rel_net_force = np.append(rel_net_force, 0) + return self.__kinematics_computation.step(rel_net_force, net_torque) def __compute_net_force_and_torque( @@ -88,8 +106,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 vector" + assert np.any(rel_water_vel), "rel_water_vel cannot be 0 vector" + + 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, 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: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: 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 b62aaef4f..ff4829048 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 @@ -2,96 +2,86 @@ import numpy as np import pytest -from numpy.typing import NDArray -from boat_simulator.common.types import Scalar -from boat_simulator.nodes.physics_engine.kinematics_data import KinematicsData +from boat_simulator.common.constants import BOAT_PROPERTIES from boat_simulator.nodes.physics_engine.model import BoatState -class TestBoatState: - @pytest.mark.parametrize( - "timestep, glo_wind_vel, glo_water_vel, rudder_angle_deg, trim_tab_angle, input_kin_data", - [ - # Generic test - ( - 0.1, - np.array([-4.0, 5.5, 0.0], dtype=np.float32), - np.array([1.0, -2.0, 0.0], dtype=np.float32), - 0, - 3, - ( - np.zeros(3, dtype=np.float32), - [2.0, 3.0, 0.0], - [1.5, 0.1, 0], - [0.2, 0.0, 3.0], - [0, 0.0, 1.1], - [-0.4, 0.0, 0], - ), - ), - # Test for still water and no wind, angular velocity, or angular acceleration - ( - 0.005, - np.array([0.0, 0.0, 0.0], dtype=np.float32), - np.array([0.0, 0.0, 0.0], dtype=np.float32), - 15, - 0, - ( - [120.0, 52.7, 0.0], - [2.0, 3.0, 0.0], - [-1, 2, 0], - [0.2, 0.0, 0.08], - np.zeros(3, dtype=np.float32), - np.zeros(3, dtype=np.float32), - ), - ), - # Test for long timestep, large parameters, negative angles - ( - 8, - np.array([5.0, 9.2, 0.0], dtype=np.float32), - np.array([-4.23, 3.0, 0.0], dtype=np.float32), - -370, - -2.6, - ( - [-2000, -4000, 0.0], - [6.0, 12.0, 0.0], - [3, -5, 0], - [0.0, 0.0, 2], - [0.3, 0.0, 3], - [-0.8, 0.0, 1.1], - ), - ), - ], +@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] ) - # Tests BoatState.step() - # __compute_net_force_and_torque and __kinematics_computation.step are tested elsewhere - def test_step_boat_state( - self, - timestep: Scalar, - glo_wind_vel: NDArray, - glo_water_vel: NDArray, - rudder_angle_deg: Scalar, - trim_tab_angle: Scalar, - input_kin_data: NDArray, - ): - test_boat_state = BoatState(timestep) - input_kinematics = KinematicsData(input_kin_data) - - relative_wind_vel = glo_wind_vel - input_kinematics.linear_velocity - relative_water_vel = glo_water_vel - input_kinematics.linear_velocity - - total_force, total_torque = getattr( - test_boat_state, - "_BoatState__compute_net_force_and_torque" - )( - relative_wind_vel, relative_water_vel, rudder_angle_deg, trim_tab_angle - ) - - actual_step = test_boat_state.step( - glo_wind_vel, glo_water_vel, rudder_angle_deg, trim_tab_angle - ) - expected_step = getattr(test_boat_state, "_BoatState__kinematics_computation").step( - total_force, total_torque - ) - - assert actual_step == expected_step + + 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 diff --git a/src/boat_simulator/tests/unit/nodes/physics_engine/test_model2.py b/src/boat_simulator/tests/unit/nodes/physics_engine/test_model2.py new file mode 100644 index 000000000..1a7291458 --- /dev/null +++ b/src/boat_simulator/tests/unit/nodes/physics_engine/test_model2.py @@ -0,0 +1,94 @@ +"""Tests classes and functions in boat_simulator/nodes/physics_engine/model.py""" +# Krista's tests from PR 471 +import numpy as np +import pytest +from numpy.typing import NDArray + +from boat_simulator.common.types import Scalar +from boat_simulator.nodes.physics_engine.kinematics_data import KinematicsData +from boat_simulator.nodes.physics_engine.model import BoatState + + +class TestBoatState: + @pytest.mark.parametrize( + "timestep, glo_wind_vel, glo_water_vel, rudder_angle_deg, trim_tab_angle, input_kin_data", + [ + # Generic test + ( + 0.1, + np.array([-4.0, 5.5, 0.0], dtype=np.float32), + np.array([1.0, -2.0, 0.0], dtype=np.float32), + 0, + 3, + ( + np.zeros(3, dtype=np.float32), + [2.0, 3.0, 0.0], + [1.5, 0.1, 0], + [0.2, 0.0, 3.0], + [0, 0.0, 1.1], + [-0.4, 0.0, 0], + ), + ), + # Test for still water and no wind, angular velocity, or angular acceleration + ( + 0.005, + np.array([0.0, 0.0, 0.0], dtype=np.float32), + np.array([0.0, 0.0, 0.0], dtype=np.float32), + 15, + 0, + ( + [120.0, 52.7, 0.0], + [2.0, 3.0, 0.0], + [-1, 2, 0], + [0.2, 0.0, 0.08], + np.zeros(3, dtype=np.float32), + np.zeros(3, dtype=np.float32), + ), + ), + # Test for long timestep, large parameters, negative angles + ( + 8, + np.array([5.0, 9.2, 0.0], dtype=np.float32), + np.array([-4.23, 3.0, 0.0], dtype=np.float32), + -370, + -2.6, + ( + [-2000, -4000, 0.0], + [6.0, 12.0, 0.0], + [3, -5, 0], + [0.0, 0.0, 2], + [0.3, 0.0, 3], + [-0.8, 0.0, 1.1], + ), + ), + ], + ) + # Tests BoatState.step() + # __compute_net_force_and_torque and __kinematics_computation.step are tested elsewhere + def test_step_boat_state( + self, + timestep: Scalar, + glo_wind_vel: NDArray, + glo_water_vel: NDArray, + rudder_angle_deg: Scalar, + trim_tab_angle: Scalar, + input_kin_data: NDArray, + ): + test_boat_state = BoatState(timestep) + input_kinematics = KinematicsData(input_kin_data) + + relative_wind_vel = glo_wind_vel - input_kinematics.linear_velocity + relative_water_vel = glo_water_vel - input_kinematics.linear_velocity + + total_force, total_torque = getattr( + test_boat_state, "_BoatState__compute_net_force_and_torque" + )(relative_wind_vel, relative_water_vel, rudder_angle_deg, trim_tab_angle) + + actual_step = test_boat_state.step( + glo_wind_vel, glo_water_vel, rudder_angle_deg, trim_tab_angle + ) + expected_step = getattr(test_boat_state, "_BoatState__kinematics_computation").step( + total_force, total_torque + ) + + assert actual_step == expected_step