From 9d46001c9ca61a67fa0fd32b058b31efa28b348d Mon Sep 17 00:00:00 2001 From: Eric Omielan <112828024+eomielan@users.noreply.github.com> Date: Sun, 22 Sep 2024 17:55:49 -0700 Subject: [PATCH] Simulator dimension mismatches & fluid generation config (#435) * Fix issues in model and physics node * Fix dimension mismatching * Fix comments for constants * Allow wind & current generation config * Allow config of fluid cov in globals.yaml * Prevent data collection from making file when not in use * Fix formatting issues with README for ci * Update fluid generation unit tests --- .../boat_simulator/common/constants.py | 48 +++++++++---- .../data_collection/data_collection_node.py | 2 +- .../nodes/physics_engine/fluid_generation.py | 6 +- .../nodes/physics_engine/model.py | 7 +- .../physics_engine/physics_engine_node.py | 69 ++++++++++++++----- .../unit/nodes/physics_engine/test_fluids.py | 38 +++++----- src/global_launch/config/README.md | 34 ++++++++- src/global_launch/config/globals.yaml | 8 +++ 8 files changed, 149 insertions(+), 63 deletions(-) diff --git a/src/boat_simulator/boat_simulator/common/constants.py b/src/boat_simulator/boat_simulator/common/constants.py index c84127c99..682933b04 100644 --- a/src/boat_simulator/boat_simulator/common/constants.py +++ b/src/boat_simulator/boat_simulator/common/constants.py @@ -38,16 +38,34 @@ 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_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) + # A lookup table 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 + # 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 + # 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] + # A scalar representing the distance from the center of effort of the sail to the pivot point + # (in meters). + sail_dist: Scalar + # A scalar representing the distance from the center of effort of the rudder to the pivot + # point (in meters). + rudder_dist: Scalar + # A dimensionless scalar representing the drag factor of the hull as a function of the boat's + # velocity. + hull_drag_factor: Scalar + # The mass of the boat (in kilograms). + mass: Scalar + # The inertia of the boat (in kilograms-meters squared). + inertia: NDArray # Directly accessible constants @@ -88,7 +106,7 @@ 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 +# 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}, @@ -96,9 +114,9 @@ class BoatProperties: 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, + sail_dist=0.5, + rudder_dist=1.0, hull_drag_factor=0.05, - mass=200.0, - inertia=np.array([[10, 0, 0], [0, 30, 0], [0, 0, 20]], dtype=np.float32), + mass=1500.0, + inertia=np.array([[125, 0, 0], [0, 1125, 0], [0, 0, 500]], dtype=np.float32), ) diff --git a/src/boat_simulator/boat_simulator/nodes/data_collection/data_collection_node.py b/src/boat_simulator/boat_simulator/nodes/data_collection/data_collection_node.py index 38dd3ace7..71d6c615e 100644 --- a/src/boat_simulator/boat_simulator/nodes/data_collection/data_collection_node.py +++ b/src/boat_simulator/boat_simulator/nodes/data_collection/data_collection_node.py @@ -32,9 +32,9 @@ def shutdown_handler(signum: int, frame: Any) -> None: def main(args=None): rclpy.init(args=args) - node = DataCollectionNode() if is_collection_enabled(): try: + node = DataCollectionNode() # TODO Explore alternatives to using the signal library, such as ROS event handlers signal.signal(signal.SIGINT, shutdown_handler) rclpy.spin(node) diff --git a/src/boat_simulator/boat_simulator/nodes/physics_engine/fluid_generation.py b/src/boat_simulator/boat_simulator/nodes/physics_engine/fluid_generation.py index 731f290e1..f24f0eb34 100644 --- a/src/boat_simulator/boat_simulator/nodes/physics_engine/fluid_generation.py +++ b/src/boat_simulator/boat_simulator/nodes/physics_engine/fluid_generation.py @@ -12,15 +12,15 @@ class FluidGenerator: """This class provides functionality to generate velocity vectors representing fluid movements. Attributes: - `generator` (VectorGenerator): The vector generator used to generate 2D fluid velocities. + `generator` (VectorGenerator): The vector generator used to generate 3D fluid velocities. `velocity` (NDArray): The most recently generated fluid velocity vector, expressed in - meters per second (m/s). It is expected to be a 2D vector. + meters per second (m/s). It is expected to be a 3D vector. """ def __init__(self, generator: VectorGenerator): self.__generator = generator self.__velocity = np.array(self.__generator.next()) - assert self.__velocity.shape == (2,) + assert self.__velocity.shape == (3,) def next(self) -> NDArray: """Generates the next velocity vector for the fluid simulation. 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 f4e5f87da..313d38746 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,7 @@ import numpy as np from numpy.typing import NDArray -from boat_simulator.common.constants import BoatProperties +from boat_simulator.common.constants import BOAT_PROPERTIES from boat_simulator.common.types import Scalar from boat_simulator.nodes.physics_engine.kinematics_computation import BoatKinematics from boat_simulator.nodes.physics_engine.kinematics_data import KinematicsData @@ -28,7 +28,7 @@ def __init__(self, timestep: Scalar): timestep (Scalar): The time interval for calculations, expressed in seconds (s). """ self.__kinematics_computation = BoatKinematics( - timestep, BoatProperties.mass, BoatProperties.inertia + timestep, BOAT_PROPERTIES.mass, BOAT_PROPERTIES.inertia ) def step( @@ -88,7 +88,8 @@ 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() + # TODO Implement this function + return (np.array([0, 0, 0]), np.array([0, 0, 0])) @property def global_position(self) -> NDArray: diff --git a/src/boat_simulator/boat_simulator/nodes/physics_engine/physics_engine_node.py b/src/boat_simulator/boat_simulator/nodes/physics_engine/physics_engine_node.py index ab0106d7b..2427d12dd 100644 --- a/src/boat_simulator/boat_simulator/nodes/physics_engine/physics_engine_node.py +++ b/src/boat_simulator/boat_simulator/nodes/physics_engine/physics_engine_node.py @@ -2,6 +2,7 @@ """The ROS node for the physics engine.""" +import json import sys from typing import Optional @@ -92,8 +93,8 @@ def __init__(self, multithreading_enabled: bool): self.__is_multithreading_enabled = multithreading_enabled self.get_logger().debug("Initializing node...") - self.__init_private_attributes() self.__declare_ros_parameters() + self.__init_private_attributes() self.__init_callback_groups() self.__init_subscriptions() self.__init_publishers() @@ -102,24 +103,6 @@ def __init__(self, multithreading_enabled: bool): self.get_logger().debug("Node initialization complete. Starting execution...") # INITIALIZATION HELPERS - def __init_private_attributes(self): - """Initializes the private attributes of this class that are not set anywhere else during - the initialization process. - """ - self.__publish_counter = 0 - self.__rudder_angle = 0 - self.__sail_trim_tab_angle = 0 - self.__desired_heading = None - self.__boat_state = BoatState( - 0.5, Constants.BOAT_PROPERTIES.mass, Constants.BOAT_PROPERTIES.inertia - ) - self.__wind_generator = FluidGenerator( - generator=MVGaussianGenerator(np.array([5, 5]), np.array([[2, 1], [1, 2]])) - ) - self.__current_generator = FluidGenerator( - generator=MVGaussianGenerator(np.array([1, 1]), np.array([[2, 1], [1, 2]])) - ) - def __declare_ros_parameters(self): """Declares ROS parameters from the global configuration file that will be used in this node. This node will monitor for any changes to these parameters during execution and will @@ -142,6 +125,10 @@ def __declare_ros_parameters(self): ("wind_sensor.gaussian_params.std_dev", rclpy.Parameter.Type.DOUBLE_ARRAY), ("wind_sensor.gaussian_params.corr_xy", rclpy.Parameter.Type.DOUBLE), ("wind_sensor.constant_params.value", rclpy.Parameter.Type.DOUBLE_ARRAY), + ("wind_generation.mvgaussian_params.mean", rclpy.Parameter.Type.DOUBLE_ARRAY), + ("wind_generation.mvgaussian_params.cov", rclpy.Parameter.Type.STRING), + ("current_generation.mvgaussian_params.mean", rclpy.Parameter.Type.DOUBLE_ARRAY), + ("current_generation.mvgaussian_params.cov", rclpy.Parameter.Type.STRING), ], ) @@ -151,6 +138,50 @@ def __declare_ros_parameters(self): value_str = str(parameter.value) self.get_logger().debug(f"Got parameter {name} with value {value_str}") + def __init_private_attributes(self): + """Initializes the private attributes of this class that are not set anywhere else during + the initialization process. + """ + self.__publish_counter = 0 + self.__rudder_angle = 0 + self.__sail_trim_tab_angle = 0 + self.__desired_heading = None + self.__boat_state = BoatState(self.pub_period) + + wind_mean = np.array( + self.get_parameter("wind_generation.mvgaussian_params.mean") + .get_parameter_value() + .double_array_value + ) + # Parse the covariance matrix from a string into a 2D array, as ROS parameters do not + # support native 2D array types. + wind_cov = np.array( + json.loads( + self.get_parameter("wind_generation.mvgaussian_params.cov") + .get_parameter_value() + .string_value + ) + ) + self.__wind_generator = FluidGenerator(generator=MVGaussianGenerator(wind_mean, wind_cov)) + + current_mean = np.array( + self.get_parameter("current_generation.mvgaussian_params.mean") + .get_parameter_value() + .double_array_value + ) + # Parse the covariance matrix from a string into a 2D array, as ROS parameters do not + # support native 2D array types. + current_cov = np.array( + json.loads( + self.get_parameter("current_generation.mvgaussian_params.cov") + .get_parameter_value() + .string_value + ) + ) + self.__current_generator = FluidGenerator( + generator=MVGaussianGenerator(current_mean, current_cov) + ) + def __init_callback_groups(self): """Initializes the callback groups. Whether multithreading is enabled or not will affect how callbacks are executed. diff --git a/src/boat_simulator/tests/unit/nodes/physics_engine/test_fluids.py b/src/boat_simulator/tests/unit/nodes/physics_engine/test_fluids.py index f7d260a99..c9ff77711 100644 --- a/src/boat_simulator/tests/unit/nodes/physics_engine/test_fluids.py +++ b/src/boat_simulator/tests/unit/nodes/physics_engine/test_fluids.py @@ -11,9 +11,9 @@ class TestFluidGenerator: @pytest.mark.parametrize( "vector", [ - (np.array([1, 0])), - (np.array([0, 1])), - (np.array([1, 0])), + (np.array([1, 0, 1])), + (np.array([0, 1, 0])), + (np.array([1, 0, 0])), ], ) def test_velocity_constant(self, vector): @@ -26,10 +26,10 @@ def test_velocity_constant(self, vector): @pytest.mark.parametrize( "mean, cov", [ - (np.array([1, 2]), np.array([[2, 1], [1, 2]])), - (np.array([4, 5]), np.array([[3, 1], [1, 3]])), - (np.array([100, 50]), np.array([[10, 5], [5, 10]])), - (np.array([120, 130]), np.array([[10, 5], [5, 10]])), + (np.array([1, 2, 0]), np.array([[2, 1, 1], [1, 2, 2], [3, 1, 2]])), + (np.array([4, 5, 3]), np.array([[3, 1, 2], [1, 3, 2], [1, 2, 2]])), + (np.array([100, 50, 20]), np.array([[10, 5, 5], [5, 10, 1], [6, 2, 5]])), + (np.array([120, 130, 40]), np.array([[10, 5, 0], [5, 10, 2], [1, 3, 5]])), ], ) def test_velocity_random(self, mean, cov): @@ -42,12 +42,12 @@ def test_velocity_random(self, mean, cov): @pytest.mark.parametrize( "vector", [ - (np.array([1, 0])), - (np.array([0, 1])), - (np.array([-1, 0])), - (np.array([0, -1])), - (np.array([1, 1])), - (np.array([-1, -1])), + (np.array([1, 0, 1])), + (np.array([0, 1, 0])), + (np.array([-1, 0, 1])), + (np.array([0, -1, 0])), + (np.array([1, 1, 1])), + (np.array([-1, -1, -1])), ], ) def test_speed(self, vector): @@ -60,12 +60,12 @@ def test_speed(self, vector): @pytest.mark.parametrize( "vector, expected_direction", [ - (np.array([1, 0]), 0), - (np.array([0, 1]), 90), - (np.array([-1, 0]), -180), - (np.array([0, -1]), -90), - (np.array([1, 1]), 45), - (np.array([-1, -1]), -135), + (np.array([1, 0, 1]), 0), + (np.array([0, 1, -3]), 90), + (np.array([-1, 0, -1]), -180), + (np.array([0, -1, 0]), -90), + (np.array([1, 1, 4]), 45), + (np.array([-1, -1, 6]), -135), ], ) def test_direction(self, vector, expected_direction): diff --git a/src/global_launch/config/README.md b/src/global_launch/config/README.md index 60065e737..e547e33c1 100644 --- a/src/global_launch/config/README.md +++ b/src/global_launch/config/README.md @@ -215,6 +215,12 @@ value. Otherwise, the trim tab angle is determined by the wingsail controller. - _Datatype_: `double` - _Range_: `(0.0, MAX_DOUBLE)` +**`wingsail.actuation_request_period_sec`** + +- _Description_: How often the sail action server requests a wingsail actuation. +- _Datatype_: `double` +- _Range_: `(0.0, MAX_DOUBLE)` + **`wind_sensor.constant_params.value`** - _Description_: Specifies the constant vector returned by the constant generator that represents the wind velocity in kmph. @@ -252,10 +258,32 @@ specified within an array: one for the `x` component, and one for the `y` compon - _Datatype_: `string` - _Acceptable Values_: `gaussian`, `constant` -**`wingsail.actuation_request_period_sec`** +**`wind_generation.mvgaussian_params.mean`** -- _Description_: How often the sail action server requests a wingsail actuation. -- _Datatype_: `double` +- _Description_: The mean value for the wind generated, expressed in kilometers per hour (km/h), for the multivariate +Gaussian generator. +- _Datatype_: `double` array, length 3 +- _Range_: `(0.0, MAX_DOUBLE)` + +**`wind_generation.mvgaussian_params.cov`** + +- _Description_: The covariance matrix for the generated wind, represented as a string formatted as a 2D `double` array, +since ROS parameters do not support native 2D array types. +- _Datatype_: `string` +- _Range_: `(0.0, MAX_DOUBLE)` + +**`current_generation.mvgaussian_params.mean`** + +- _Description_: The mean value for the current generated, expressed in kilometers per hour (km/h), for the multivariate +Gaussian generator. +- _Datatype_: `double` array, length 3 +- _Range_: `(0.0, MAX_DOUBLE)` + +**`current_generation.mvgaussian_params.cov`** + +- _Description_: The covariance matrix for the generated current, represented as a string formatted as a 2D `double` +array, since ROS parameters do not support native 2D array types. +- _Datatype_: `string` - _Range_: `(0.0, MAX_DOUBLE)` ### `data_collection_node` diff --git a/src/global_launch/config/globals.yaml b/src/global_launch/config/globals.yaml index 7cec8befa..5370d2e33 100644 --- a/src/global_launch/config/globals.yaml +++ b/src/global_launch/config/globals.yaml @@ -60,6 +60,14 @@ physics_engine_node: corr_xy: 0. constant_params: value: [1.0, 0.0] + wind_generation: + mvgaussian_params: + mean: [5.0, 5.0, 0.0] + cov: "[[25.0, 10.0, 5.0], [10.0, 15.0, 2.0], [5.0, 2.0, 20.0]]" + current_generation: + mvgaussian_params: + mean: [1.0, 0.5, 0.0] + cov: "[[0.5, 0.1, 0.05], [0.1, 0.3, 0.02], [0.05, 0.02, 0.2]]" data_collection_node: ros__parameters: file_name: 'ros_data_collection'