Run batch simulations over set of startup params in yaml file

buoy_description/CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(ament_cmake REQUIRED)
 set(BUOY_BASE_MODEL_PATH "/models/mbari_wec_base")
 file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}${BUOY_BASE_MODEL_PATH})
 add_custom_command(
-  OUTPUT model_gen_cmd
+  OUTPUT base_model_gen_cmd
   COMMAND python3 -m em
     ${CMAKE_CURRENT_SOURCE_DIR}${BUOY_BASE_MODEL_PATH}/model.sdf.em >
     ${CMAKE_CURRENT_BINARY_DIR}${BUOY_BASE_MODEL_PATH}/model.sdf
@@ -16,6 +16,17 @@ file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}${BUOY_MODEL_PATH})
 set(BUOY_ROS_MODEL_PATH "/models/mbari_wec_ros")
 file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}${BUOY_ROS_MODEL_PATH})
 
+add_custom_target(base_model_gen_target ALL
+  DEPENDS base_model_gen_cmd
+)
+
+add_custom_command(
+  OUTPUT model_gen_cmd
+  COMMAND python3 -m em
+    ${CMAKE_CURRENT_SOURCE_DIR}${BUOY_MODEL_PATH}/model.sdf.em >
+    ${CMAKE_CURRENT_BINARY_DIR}${BUOY_MODEL_PATH}/model.sdf
+)
+
 add_custom_target(model_gen_target ALL
   DEPENDS model_gen_cmd
 )

buoy_description/models/mbari_wec/model.sdf → buoy_description/models/mbari_wec/model.sdf.em

@@ -1,4 +1,11 @@
 <?xml version="1.0" ?>
+@{
+try:
+    scale_factor
+except NameError:
+    scale_factor = 1.0  # not defined so default
+}@
+
 <sdf version="1.8">
   <model name="MBARI_WEC">
 
@@ -11,6 +18,7 @@
       <PistonArea>1.375</PistonArea>
       <HydMotorDisp>0.30</HydMotorDisp>
       <RotorInertia>1</RotorInertia>
+      <ScaleFactor>@(scale_factor)</ScaleFactor>
     </plugin>
 
     <!-- Upper Polytropic Spring plugin -->

buoy_description/models/mbari_wec_base/model.sdf.em

@@ -27,13 +27,37 @@ tether_top_length = 2.5
 
 num_tether_bottom_links = 5
 
-# Heave cone
-heave_total_mass = 817
-trefoil_mass = 10
+# TrefoilDoors
+trefoil_pose = {'open': 1.047, 'closed': 0.0}
+# check if door_state was passed in by empy
+try:
+    door_state
+except NameError:
+    door_state = 'closed'  # not defined so default closed
+
+if 'open' in door_state:
+    mu_zz = 3000.0  # kg, Heave Added Mass
+    z_ww = -3200.0  # kg/m, Heave Quadratic Drag
 
 # Heave cone
-heave_total_mass = 817
-trefoil_mass = 10
+heave_total_mass = 817  # kg
+trefoil_mass = 20  # kg
+
+########################
+# TODO(anyone) mu_zz and z_ww below are currently unused. Will be used in #115
+########################
+
+# check if mu_zz was set by door_state 'open' (or passed in by empy)
+try:
+    mu_zz
+except NameError:
+    mu_zz = 10000.0  # kg, not defined so default with doors closed
+
+# check if z_ww was set by door_state 'open' (or passed in by empy)
+try:
+    z_ww
+except NameError:
+    z_ww = -3900.0  # kg/m, not defined so default with doors closed
 
 ###################
 # Computed values #
@@ -449,18 +473,11 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
       </axis>
     </joint>
 
-    <joint name="TrefoilDoors" type="revolute">
+    <joint name="TrefoilDoors" type="fixed">
       <parent>HeaveCone</parent>
       <child>Trefoil</child>
       <provide_feedback>1</provide_feedback>
-      <pose>0.0 0.0 0.0 0 0 0</pose>
-      <axis>
-        <limit>
-          <lower>0.0</lower>
-          <upper>1.047</upper>
-        </limit>
-        <xyz>0.0 0.0 1.0</xyz>
-      </axis>
+      <pose>0.0 0.0 0.0 0 0 @(trefoil_pose[door_state])</pose>
     </joint>
 
   </model>

buoy_gazebo/CMakeLists.txt

@@ -143,6 +143,10 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+# Install batch running script
+install(PROGRAMS scripts/mbari_wec_batch DESTINATION lib/${PROJECT_NAME}/)
+install(FILES scripts/example_sim_params.yaml DESTINATION share/${PROJECT_NAME})
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

buoy_gazebo/launch/mbari_wec.launch.py

@@ -61,12 +61,16 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'),
         ),
-        launch_arguments={'gz_args': PathJoinSubstitution([
-            pkg_buoy_gazebo,
-            'worlds',
-            LaunchConfiguration('world_file')
-        ]),
-            'debugger': LaunchConfiguration('debugger')}.items(),
+        launch_arguments={'gz_args': [
+            LaunchConfiguration('gz_args'), PathJoinSubstitution([' ']),
+            PathJoinSubstitution([
+                pkg_buoy_gazebo,
+                'worlds',
+                LaunchConfiguration('world_file')
+                ])
+            ],
+            'debugger': LaunchConfiguration('debugger'),
+            'on_exit_shutdown': 'True'}.items(),
     )
 
     # Bridge to forward tf and joint states to ros2

buoy_gazebo/launch/mbari_wec_batch.launch.py

@@ -0,0 +1,44 @@
+# Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Launch Gazebo world with a buoy."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, Shutdown
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    sim_params_yaml_launch_arg = DeclareLaunchArgument(
+        'sim_params_yaml',
+        description='Input batch sim params yaml'
+    )
+
+    batch_sim = Node(
+        package='buoy_gazebo',
+        executable='mbari_wec_batch',
+        arguments=[
+            LaunchConfiguration('sim_params_yaml'),
+        ],
+        output='screen',
+        on_exit=Shutdown()
+    )
+
+    return LaunchDescription([
+        sim_params_yaml_launch_arg,
+        batch_sim,
+    ])

buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py

@@ -66,7 +66,8 @@ def generate_launch_description():
             'worlds',
             LaunchConfiguration('world_file')
         ]),
-            'debugger': LaunchConfiguration('debugger')}.items(),
+            'debugger': LaunchConfiguration('debugger'),
+            'on_exit_shutdown': 'True'}.items(),
     )
 
     # Bridge to forward tf and joint states to ros2

buoy_gazebo/scripts/example_sim_params.yaml

@@ -0,0 +1,4 @@
+seed: 42
+duration: 5
+door_state: ['closed', 'open']
+scale_factor: [0.5, 0.625, 0.75, 0.875, 1.0, 1.125, 1.25, 1.375, 1.4]