Run batch simulations over set of startup params in yaml file (#125)

* proof of concept for looping over simulation parameters Signed-off-by: Michael Anderson <[email protected]> * try to fix Signed-off-by: Michael Anderson <[email protected]> * set on_exit shutdown in launch files Signed-off-by: Michael Anderson <[email protected]> * install script and example batch sim params; add launch file; set default to use example sim params Signed-off-by: Michael Anderson <[email protected]> * add rosbag and a log file; rename some things Signed-off-by: Michael Anderson <[email protected]> * fix max scale in example yaml Signed-off-by: Michael Anderson <[email protected]> * typo Signed-off-by: Michael Anderson <[email protected]> * remove noisy prints; get tests passing Signed-off-by: Michael Anderson <[email protected]> * use argparse Signed-off-by: Michael Anderson <[email protected]> * mbari_wec/model.sdf is now generated by empy Signed-off-by: Michael Anderson <[email protected]> * try to debug pc commands test Signed-off-by: Michael Anderson <[email protected]> * debug pc commands test Signed-off-by: Michael Anderson <[email protected]> * increase timeout; remove debugging prints Signed-off-by: Michael Anderson <[email protected]> * keep debugging Signed-off-by: Michael Anderson <[email protected]> * keep debugging Signed-off-by: Michael Anderson <[email protected]> * keep debugging Signed-off-by: Michael Anderson <[email protected]> * remove debugging code Signed-off-by: Michael Anderson <[email protected]> * set z_ww for doors open/closed -- unused until #115 Signed-off-by: Michael Anderson <[email protected]> * fix argparse Signed-off-by: Michael Anderson <[email protected]> * cleanup prints; remove default arg for batch launch Signed-off-by: Michael Anderson <[email protected]> * change iterations to duration in batch params yaml Signed-off-by: Michael Anderson <[email protected]> * clean up Signed-off-by: Michael Anderson <[email protected]> * clean up Signed-off-by: Michael Anderson <[email protected]> * added plumbing for random seed but not yet supported by our version of gz sim Signed-off-by: Michael Anderson <[email protected]> * make new directory for batch results; copy params yaml to results; timestamp everything; put rosbag names in log Signed-off-by: Michael Anderson <[email protected]> * add some info prints Signed-off-by: Michael Anderson <[email protected]> * flake8 Signed-off-by: Michael Anderson <[email protected]> * error checking; let --seed be optional and other params default; duration required Signed-off-by: Michael Anderson <[email protected]> * connect batching template to viscous drag plugin Signed-off-by: Michael Anderson <[email protected]> * retest until pass Signed-off-by: Michael Anderson <[email protected]> --------- Signed-off-by: Michael Anderson <[email protected]>
osrf · Jan 31, 2023 · bc7e914 · bc7e914
1 parent cb888d0
commit bc7e914
Show file tree

Hide file tree

Showing 19 changed files with 349 additions and 35 deletions.
diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh
@@ -51,5 +51,5 @@ colcon build --packages-up-to buoy_tests --event-handlers console_direct+
 source $COLCON_WS/install/setup.bash
 
 # Test all buoy packages
-colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+
+colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ --retest-until-pass 10
 colcon test-result
diff --git a/buoy_description/CMakeLists.txt b/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
 )

diff --git a/buoy_description/models/mbari_wec/model.sdf → ...description/models/mbari_wec/model.sdf.em b/buoy_description/models/mbari_wec/model.sdf → ...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">
 
@@ -12,6 +19,7 @@
       <PistonArea>1.375</PistonArea>
       <HydMotorDisp>0.30</HydMotorDisp>
       <RotorInertia>1</RotorInertia>
+      <ScaleFactor>@(scale_factor)</ScaleFactor>
     </plugin>
 
     <!-- Upper Polytropic Spring plugin -->

diff --git a/buoy_description/models/mbari_wec_base/model.sdf.em b/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
+    heavecone_zWabsW = -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 below is 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:
+    heavecone_zWabsW
+except NameError:
+    heavecone_zWabsW = -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>
 
     <!-- Viscous Drag for Buoy -->
@@ -497,7 +514,8 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
       <link_name>HeaveCone</link_name>
       <xUabsU>-1580</xUabsU>  <!-- Surge Quadratic Drag kg/m -->
       <yVabsV>-1580</yVabsV>  <!-- Sway Quadratic Drag kg/m -->
-      <zWabsW>-3900</zWabsW>  <!-- Vertical Quadratic Drag kg/m: -3200 open, -3900 close -->
+      <!-- Vertical Quadratic Drag kg/m: -3200 open, -3900 close -->
+      <zWabsW>@(heavecone_zWabsW)</zWabsW>
       <kPabsP>-4620</kPabsP>  <!-- Roll Quadratic Drag kg m^2 -->
       <mQabsQ>-4620</mQabsQ>  <!-- Pitch Quadratic Drag kg m^2 -->
       <nRabsR>-50</nRabsR>  <!-- Yaw Quadratic Drag kg m^2 -->

diff --git a/buoy_gazebo/CMakeLists.txt b/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()

diff --git a/buoy_gazebo/launch/mbari_wec.launch.py b/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

diff --git a/buoy_gazebo/launch/mbari_wec_batch.launch.py b/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,
+    ])
diff --git a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py b/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

diff --git a/buoy_gazebo/scripts/example_sim_params.yaml b/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]