From c20e586362f7dc092a609eaa821950cf4d709087 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 17 Nov 2021 08:46:12 +0800 Subject: [PATCH 01/43] rigid body Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf | 59 ++++++++++--------- .../worlds/buoyant_tethys.sdf | 13 +--- 2 files changed, 33 insertions(+), 39 deletions(-) diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf index fd854274..376dc019 100644 --- a/lrauv_description/models/tethys/model.sdf +++ b/lrauv_description/models/tethys/model.sdf @@ -11,23 +11,23 @@ - 114.8364 + + 147.5671 - - + 41.980233 - 2.4 + @@ -37,6 +37,7 @@ 2 0.3 0.245945166667 + @@ -72,7 +73,7 @@ - + - + - + - - + - 31.7307 + - 0.6489446 0.24638 0.17415 + - + - 0.4 0 0 0 0 0 + - + - + - + - + diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 202555be..a7c8bedf 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -5,6 +5,7 @@ --> + 0 0 -9.8 0.0 1.0 1.0 @@ -53,16 +54,8 @@ 1 + 1000 - - - - - - + -0.1181 0 0 0 0 0 147.5671 @@ -33,7 +33,7 @@ - 0 0 0.007 0 0 0 + -0.1181 0 0.007 0 0 0 2 0.3 0.245945166667 From 14f8267931b9b01b2c1ffc1bb67d8209834da975 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 17 Nov 2021 09:19:42 +0800 Subject: [PATCH 03/43] Add drop weights... Leads to oscillation Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf | 6 +- .../models/tethys_equipped/model.sdf | 56 +++++++++---------- .../worlds/buoyant_tethys.sdf | 2 +- 3 files changed, 32 insertions(+), 32 deletions(-) diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf index f46967bd..268247c1 100644 --- a/lrauv_description/models/tethys/model.sdf +++ b/lrauv_description/models/tethys/model.sdf @@ -12,7 +12,7 @@ -0.1181 0 0 0 0 0 - 147.5671 + 146.5671 3.000000 @@ -376,7 +376,7 @@ --> - + diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf index d309c7ea..aaed5dff 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -9,7 +9,7 @@ tethys - + - horizontal_fins_joint @@ -104,10 +104,10 @@ battery_joint true 0.0007 - + --> - 1000 @@ -117,16 +117,16 @@ 0.03 0.17 0 - 0.0244 + 0.0244--> - 0 1 0 + - -1 0 0 + - 1000 @@ -136,24 +136,24 @@ 0.03 0.17 0 - 0.0244 + 0.0244--> - 0 0 1 + - -1 0 0 + - tethys tethys/command_topic tethys/state_topic 0 - - --> + + - 0.000080 + - 0.0005 - 0.0005 + + - 0.000955 + - 0.000003 - + @@ -202,7 +202,7 @@ /model/tethys/drop_weight -
1
@@ -221,7 +221,7 @@ 1500 base_link tethys - + --> diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index a7c8bedf..87ef1e0b 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -299,7 +299,7 @@ 0 0 0 0 0 0 - tethys + tethys_equipped From 0f271f2b7b5056ee31500d10580c433ed130bfd4 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 17 Nov 2021 15:19:37 +0800 Subject: [PATCH 04/43] shift COM of main body forward craft is now stable. Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf index 268247c1..ad36c422 100644 --- a/lrauv_description/models/tethys/model.sdf +++ b/lrauv_description/models/tethys/model.sdf @@ -9,7 +9,7 @@ - -0.1181 0 0 0 0 0 + -0.119779049 0 0 0 0 0 146.5671 @@ -377,16 +377,12 @@ - 0 0 0 0 0 0 + 0.128 0 -0.142 0 0 0 - 0 0 0 0 0 0 1 0.000143971303 - 0.000000000008 - -0.000000000224 0.000140915448 - -0.000025236433 0.000033571862 From 53a4415587981d22a5a8e47cf7871a608ff57d7a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 17 Nov 2021 16:37:37 +0800 Subject: [PATCH 05/43] Added mass shifter craaazy oscillations are back Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf | 25 ++++++++++++----------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf index ad36c422..77b6bcc1 100644 --- a/lrauv_description/models/tethys/model.sdf +++ b/lrauv_description/models/tethys/model.sdf @@ -11,8 +11,8 @@ -0.119779049 0 0 0 0 0 - - 146.5671 + 114.8364 + 3.000000 @@ -239,13 +239,14 @@ - + + + + - + - + @@ -360,7 +361,7 @@ --> - + From d28890fd961d3e6a4482d29f55a4e7656f679da5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 19 Nov 2021 17:33:07 +0800 Subject: [PATCH 06/43] Add description generator script Signed-off-by: Arjo Chakravarty --- .../models/tethys/{model.sdf => model.sdf.in} | 14 +- .../scripts/description_generator.py | 121 ++++++++++++++++++ 2 files changed, 128 insertions(+), 7 deletions(-) rename lrauv_description/models/tethys/{model.sdf => model.sdf.in} (94%) create mode 100644 lrauv_description/scripts/description_generator.py diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf.in similarity index 94% rename from lrauv_description/models/tethys/model.sdf rename to lrauv_description/models/tethys/model.sdf.in index 77b6bcc1..f7d66fc2 100644 --- a/lrauv_description/models/tethys/model.sdf +++ b/lrauv_description/models/tethys/model.sdf.in @@ -9,9 +9,9 @@ - -0.119779049 0 0 0 0 0 + @calculated - 114.8364 + @calculated @@ -33,13 +33,13 @@ - -0.1181 0 0.007 0 0 0 + - + --> + @calculated @@ -361,7 +361,7 @@ --> - + diff --git a/lrauv_description/scripts/description_generator.py b/lrauv_description/scripts/description_generator.py new file mode 100644 index 00000000..f20b137e --- /dev/null +++ b/lrauv_description/scripts/description_generator.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2021 Open Source Robotics Foundation +# +# 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. + +# Development of this module has been funded by the Monterey Bay Aquarium +# Research Institute (MBARI) and the David and Lucile Packard Foundation + +# Usage: +# description_generator.py +# This file takes in the model.sdf.in file and generates an output model.sdf. +# The aim is to produce a perfectly stable model, despite of adding control +# elements. + +import xml.etree.ElementTree as ET +import numpy as np +import sys + +## These are parameters of the WHOLE VEHICLE +# TODO(arjo): Implement inertia. Need to implement Inertial::operator-(const Inertial&) first +total_mass = 147.5671 # Total mass of the vehicle +buoyancy_z_offset = 0.007 # Buoyancy offset +fluid_density = 1000 #fluid density + +def read_pose(element): + """ Read pose element.""" + return [float(token) for token in element.text.split()] + +def write_float_array(pose): + res = "" + for p in pose: + res += str(p) + " " + return res + +def calculate_center_of_mass(total_mass, template_path, output_path): + + sdf_file_input = ET.parse(template_path) + + for sdf in sdf_file_input.iter("sdf"): + for model in sdf.iter("model"): + moments = [] + main_body_com_tag = None + main_body_com_pose = None + collision_tag = None + ## Get all links + for links in model.iter("link"): + link_pose = links.find(".pose") + if link_pose is None: + center_of_mass = [0] * 6 + else: + # TODO(arjo) generate errors, perform protection + center_of_mass = [float(token) for token in link_pose.text.split()] + + ## Get inertial value of part + inertia_element = links.find(".inertial") + collision_element = links.find(".collision") + + ## Calculate buoyancy + if collision_element is not None: + if collision_element.text.strip() == "@calculated": + collision_tag = collision_element + else: + # TODO(arjo): Calculate moments arising from collision + pass + + ## Calculate moments due to inertia + if inertia_element is not None: + # TODO(arjo): Just put a @calculated on the whole inertia itself + mass = links.find(".inertial/mass") + pose = links.find(".inertial/pose") + if mass.text.strip() == "@calculated": + main_body_com_tag = mass + main_body_com_pose = pose + continue + else: + # add the moment into our moment list + if pose is not None: + center_of_mass = [float(token) for token in pose.text.split()] + assert len(center_of_mass) == 6 + moments.append((float(mass.text), np.array(center_of_mass))) + + # get the moments in the model + total_moment = sum([mass * com for mass, com in moments]) + component_mass = sum([mass for mass, _ in moments]) + remaining_mass = total_mass - component_mass + main_body_com = - total_moment / remaining_mass + + main_body_com_tag.text = str(remaining_mass) + main_body_com_pose.text = write_float_array(main_body_com) + + # calculate buoyancy position + cube_length = total_mass / (2 * 0.2 * fluid_density) + collision_tag.text = "" + pose_tag = ET.SubElement(collision_tag, "pose") + pose_tag.text = write_float_array([0, 0, buoyancy_z_offset, 0, 0, 0]) + + geometry = ET.SubElement(collision_tag, "geometry") + box = ET.SubElement(geometry, "box") + size = ET.SubElement(box, "size") + size.text = write_float_array([2, 0.2, cube_length]) + + sdf_file_input.write(output_path) + +if __name__ == "__main__": + if len(sys.argv) != 3: + print("Usage:") + print("description_generator.py ") + exit(-100) + + calculate_center_of_mass(total_mass, sys.argv[1], sys.argv[2]) From 92098f76c04b46169c323508703a0508c7fdc6a7 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 20 Nov 2021 14:23:59 +0800 Subject: [PATCH 07/43] Automate pipeline Signed-off-by: Arjo Chakravarty --- lrauv_description/CMakeLists.txt | 10 ++ lrauv_description/models/tethys/model.sdf | 127 ++++++++++++++++++ .../scripts/description_generator.py | 8 +- 3 files changed, 144 insertions(+), 1 deletion(-) create mode 100644 lrauv_description/models/tethys/model.sdf diff --git a/lrauv_description/CMakeLists.txt b/lrauv_description/CMakeLists.txt index 664bdab7..de0e472c 100644 --- a/lrauv_description/CMakeLists.txt +++ b/lrauv_description/CMakeLists.txt @@ -12,7 +12,17 @@ configure_file( "${CMAKE_CURRENT_BINARY_DIR}/hooks/hook.dsv" @ONLY ) +add_custom_command( + OUTPUT modelsdf + COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/scripts/description_generator.py ${CMAKE_CURRENT_SOURCE_DIR}/models/tethys/model.sdf.in ${CMAKE_CURRENT_BINARY_DIR}/models/tethys/model.sdf +) + +add_custom_target(generate_model ALL + DEPENDS modelsdf +) + install(DIRECTORY models + ${CMAKE_CURRENT_BINARY_DIR}/models/tethys/model.sdf ${CMAKE_CURRENT_BINARY_DIR}/hooks DESTINATION share/${PROJECT_NAME}) diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf new file mode 100644 index 00000000..73803192 --- /dev/null +++ b/lrauv_description/models/tethys/model.sdf @@ -0,0 +1,127 @@ + + + + + + + 0.03198178513175526 -0.0 0.0012365417237043305 -0.0 -0.0 -0.0 + + 114.83640000000001 + + + + 3.000000 + 0 + 0 + 41.980233 + 0 + 41.980233 + + + + + + + 0.03198178513175526 -0.0 0.007 0 0 0 2 0.2 0.36891775000000004 + + + + + meshes/tethys.dae + + Body +
false
+ + + + + 1.0 1.0 1.0 1.0 + 1.0 1.0 1.0 1.0 + + + materials/textures/Tethys_Albedo.png + materials/textures/Tethys_Normal.png + materials/textures/Tethys_Metalness.png + materials/textures/Tethys_Roughness.png + + + + + + + + + + + + + + + + + + + + + + + + + -0.119779049 0 0 0 0 0 + 31.7307 + + 0.2327 + 0 + -0.0004 + 1.0930 + 0 + 1.0344 + + + + + + + + 0.6489446 0.24638 0.17415 + + + + 0.0 0.0 1.0 1.0 + 0.0 0.0 1.0 1.0 + + + + + + + + + + + + + + + + + + + + 0.128 0 -0.142 0 0 0 + + 1 + + 0.000143971303 + 0.000140915448 + 0.000033571862 + + + + + + \ No newline at end of file diff --git a/lrauv_description/scripts/description_generator.py b/lrauv_description/scripts/description_generator.py index f20b137e..664a40c8 100644 --- a/lrauv_description/scripts/description_generator.py +++ b/lrauv_description/scripts/description_generator.py @@ -26,6 +26,8 @@ import xml.etree.ElementTree as ET import numpy as np import sys +import os +import os.path as path ## These are parameters of the WHOLE VEHICLE # TODO(arjo): Implement inertia. Need to implement Inertial::operator-(const Inertial&) first @@ -103,13 +105,17 @@ def calculate_center_of_mass(total_mass, template_path, output_path): cube_length = total_mass / (2 * 0.2 * fluid_density) collision_tag.text = "" pose_tag = ET.SubElement(collision_tag, "pose") - pose_tag.text = write_float_array([0, 0, buoyancy_z_offset, 0, 0, 0]) + pose_tag.text = write_float_array([main_body_com[0], main_body_com[1], buoyancy_z_offset, 0, 0, 0]) geometry = ET.SubElement(collision_tag, "geometry") box = ET.SubElement(geometry, "box") size = ET.SubElement(box, "size") size.text = write_float_array([2, 0.2, cube_length]) + dir_name = path.dirname(output_path) + if not path.exists(dir_name): + os.makedirs(dir_name) + sdf_file_input.write(output_path) if __name__ == "__main__": From 580ddff2311c1f66258111317c8d8469db8b266d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 20 Nov 2021 15:05:31 +0800 Subject: [PATCH 08/43] we are generating a new sdf Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf | 127 ---------------------- 1 file changed, 127 deletions(-) delete mode 100644 lrauv_description/models/tethys/model.sdf diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf deleted file mode 100644 index 73803192..00000000 --- a/lrauv_description/models/tethys/model.sdf +++ /dev/null @@ -1,127 +0,0 @@ - - - - - - - 0.03198178513175526 -0.0 0.0012365417237043305 -0.0 -0.0 -0.0 - - 114.83640000000001 - - - - 3.000000 - 0 - 0 - 41.980233 - 0 - 41.980233 - - - - - - - 0.03198178513175526 -0.0 0.007 0 0 0 2 0.2 0.36891775000000004 - - - - - meshes/tethys.dae - - Body -
false
- - - - - 1.0 1.0 1.0 1.0 - 1.0 1.0 1.0 1.0 - - - materials/textures/Tethys_Albedo.png - materials/textures/Tethys_Normal.png - materials/textures/Tethys_Metalness.png - materials/textures/Tethys_Roughness.png - - - - - - - - - - - - - - - - - - - - - - - - - -0.119779049 0 0 0 0 0 - 31.7307 - - 0.2327 - 0 - -0.0004 - 1.0930 - 0 - 1.0344 - - - - - - - - 0.6489446 0.24638 0.17415 - - - - 0.0 0.0 1.0 1.0 - 0.0 0.0 1.0 1.0 - - - - - - - - - - - - - - - - - - - - 0.128 0 -0.142 0 0 0 - - 1 - - 0.000143971303 - 0.000140915448 - 0.000033571862 - - - - - - \ No newline at end of file From e4e1ec0c95c2b2078397db45646160d7b7aaa799 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 22 Nov 2021 09:04:36 +0800 Subject: [PATCH 09/43] Add unit test framework Signed-off-by: Arjo Chakravarty --- lrauv_description/CMakeLists.txt | 48 +++++++++++++- lrauv_description/models/tethys/model.sdf.in | 30 +++------ .../scripts/description_generator.py | 16 +++-- .../test/helper/TestConstants.hh.in | 6 ++ .../test/test_hydrostatic_stability.cc | 0 lrauv_description/test/worlds/flat_world.sdf | 63 +++++++++++++++++++ .../test/worlds/tilted_world.sdf | 63 +++++++++++++++++++ 7 files changed, 197 insertions(+), 29 deletions(-) create mode 100644 lrauv_description/test/helper/TestConstants.hh.in create mode 100644 lrauv_description/test/test_hydrostatic_stability.cc create mode 100644 lrauv_description/test/worlds/flat_world.sdf create mode 100644 lrauv_description/test/worlds/tilted_world.sdf diff --git a/lrauv_description/CMakeLists.txt b/lrauv_description/CMakeLists.txt index de0e472c..66b02327 100644 --- a/lrauv_description/CMakeLists.txt +++ b/lrauv_description/CMakeLists.txt @@ -7,11 +7,16 @@ cmake_minimum_required(VERSION 3.5) project(lrauv_description) + +#============================================================================ +# Hooks configure_file( "hooks/hook.dsv.in" "${CMAKE_CURRENT_BINARY_DIR}/hooks/hook.dsv" @ONLY ) +#============================================================================ +# Model Generation add_custom_command( OUTPUT modelsdf COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/scripts/description_generator.py ${CMAKE_CURRENT_SOURCE_DIR}/models/tethys/model.sdf.in ${CMAKE_CURRENT_BINARY_DIR}/models/tethys/model.sdf @@ -19,10 +24,49 @@ add_custom_command( add_custom_target(generate_model ALL DEPENDS modelsdf -) +) install(DIRECTORY models - ${CMAKE_CURRENT_BINARY_DIR}/models/tethys/model.sdf ${CMAKE_CURRENT_BINARY_DIR}/hooks DESTINATION share/${PROJECT_NAME}) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/models/tethys/model.sdf + DESTINATION share/${PROJECT_NAME}/models/tethys +) + +#============================================================================ +# Tests +if(BUILD_TESTING) + + find_package(ignition-gazebo6 REQUIRED COMPONENTS gui) + set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) + + # Fetch and configure GTest + include(FetchContent) + FetchContent_Declare( + googletest + URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip + ) + set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) + FetchContent_MakeAvailable(googletest) + + enable_testing() + include(Dart) + + # Build-time constants + set("PROJECT_BINARY_PATH" ${CMAKE_CURRENT_BINARY_DIR}) + set("PROJECT_SOURCE_PATH" ${CMAKE_CURRENT_SOURCE_DIR}) + configure_file(test/helper/TestConstants.hh.in TestConstants.hh @ONLY) + + include(GoogleTest) + + add_executable(test_hydrostatic_stability test/test_hydrostatic_stability.cc) + target_link_libraries(test_hydrostatic_stability + PUBLIC gtest_main + PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER} + ) + + gtest_discover_tests(test_hydrostatic_stability) +endif() \ No newline at end of file diff --git a/lrauv_description/models/tethys/model.sdf.in b/lrauv_description/models/tethys/model.sdf.in index f7d66fc2..0fd0cf8f 100644 --- a/lrauv_description/models/tethys/model.sdf.in +++ b/lrauv_description/models/tethys/model.sdf.in @@ -21,24 +21,11 @@ 41.980233 0 41.980233 - - - @calculated @@ -239,13 +226,12 @@ - + - - -0.119779049 0 0 0 0 0 + -0.563 0 0 0 0 0 31.7307 0.2327 @@ -260,7 +246,6 @@ - 0.6489446 0.24638 0.17415 @@ -361,7 +346,7 @@ --> - + - - 0.128 0 -0.142 0 0 0 + diff --git a/lrauv_description/scripts/description_generator.py b/lrauv_description/scripts/description_generator.py index 664a40c8..fc335519 100644 --- a/lrauv_description/scripts/description_generator.py +++ b/lrauv_description/scripts/description_generator.py @@ -51,7 +51,7 @@ def calculate_center_of_mass(total_mass, template_path, output_path): for sdf in sdf_file_input.iter("sdf"): for model in sdf.iter("model"): - moments = [] + moments = [(0, np.array([0,0,0,0,0,0]))] main_body_com_tag = None main_body_com_pose = None collision_tag = None @@ -94,23 +94,31 @@ def calculate_center_of_mass(total_mass, template_path, output_path): # get the moments in the model total_moment = sum([mass * com for mass, com in moments]) + + # this is the mass of the various other payloads component_mass = sum([mass for mass, _ in moments]) remaining_mass = total_mass - component_mass + print(component_mass, file=sys.stderr) main_body_com = - total_moment / remaining_mass main_body_com_tag.text = str(remaining_mass) main_body_com_pose.text = write_float_array(main_body_com) # calculate buoyancy position - cube_length = total_mass / (2 * 0.2 * fluid_density) + cube_length = total_mass / (2 * 0.3 * fluid_density) collision_tag.text = "" pose_tag = ET.SubElement(collision_tag, "pose") - pose_tag.text = write_float_array([main_body_com[0], main_body_com[1], buoyancy_z_offset, 0, 0, 0]) + pose_tag.text = write_float_array([0, 0, buoyancy_z_offset, 0, 0, 0]) geometry = ET.SubElement(collision_tag, "geometry") box = ET.SubElement(geometry, "box") size = ET.SubElement(box, "size") - size.text = write_float_array([2, 0.2, cube_length]) + size.text = write_float_array([2, 0.3, cube_length]) + + print(total_moment + main_body_com * remaining_mass, file=sys.stderr) + print(main_body_com, file=sys.stderr) + + assert sum(total_moment + main_body_com * remaining_mass) == 0 dir_name = path.dirname(output_path) if not path.exists(dir_name): diff --git a/lrauv_description/test/helper/TestConstants.hh.in b/lrauv_description/test/helper/TestConstants.hh.in new file mode 100644 index 00000000..097f2f33 --- /dev/null +++ b/lrauv_description/test/helper/TestConstants.hh.in @@ -0,0 +1,6 @@ +#ifndef __TEST_CONSTANTS_HH__ +#define __TEST_CONSTANTS_HH__ +#cmakedefine PROJECT_BINARY_PATH "@PROJECT_BINARY_PATH@" +#cmakedefine PROJECT_SOURCE_PATH "@PROJECT_SOURCE_PATH@" + +#endif diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc new file mode 100644 index 00000000..e69de29b diff --git a/lrauv_description/test/worlds/flat_world.sdf b/lrauv_description/test/worlds/flat_world.sdf new file mode 100644 index 00000000..a5a7c6a4 --- /dev/null +++ b/lrauv_description/test/worlds/flat_world.sdf @@ -0,0 +1,63 @@ + + + + + 0 0 -9.8 + + + 0.0 1.0 1.0 + + + 0.0 0.7 0.8 + + + + + 0.02 + 1 + + + + + + + + + + + + 1000 + + + + 0 0 0 0 0 0 + tethys + + + + + diff --git a/lrauv_description/test/worlds/tilted_world.sdf b/lrauv_description/test/worlds/tilted_world.sdf new file mode 100644 index 00000000..741a07b7 --- /dev/null +++ b/lrauv_description/test/worlds/tilted_world.sdf @@ -0,0 +1,63 @@ + + + + + 0 0 -9.8 + + + 0.0 1.0 1.0 + + + 0.0 0.7 0.8 + + + + + 0.02 + 0 + + + + + + + + + + - 1000 0 0 0 0 0.08 0 tethys - - base_link - __model__ - drop_weight - /model/tethys/drop_weight - From 12ae53cf8565077f262e5448491dbd6a8e463965 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 22 Nov 2021 15:46:41 +0800 Subject: [PATCH 11/43] remove model Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf | 127 ---------------------- 1 file changed, 127 deletions(-) delete mode 100644 lrauv_description/models/tethys/model.sdf diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf deleted file mode 100644 index 73803192..00000000 --- a/lrauv_description/models/tethys/model.sdf +++ /dev/null @@ -1,127 +0,0 @@ - - - - - - - 0.03198178513175526 -0.0 0.0012365417237043305 -0.0 -0.0 -0.0 - - 114.83640000000001 - - - - 3.000000 - 0 - 0 - 41.980233 - 0 - 41.980233 - - - - - - - 0.03198178513175526 -0.0 0.007 0 0 0 2 0.2 0.36891775000000004 - - - - - meshes/tethys.dae - - Body -
false
- - - - - 1.0 1.0 1.0 1.0 - 1.0 1.0 1.0 1.0 - - - materials/textures/Tethys_Albedo.png - materials/textures/Tethys_Normal.png - materials/textures/Tethys_Metalness.png - materials/textures/Tethys_Roughness.png - - - - - - - - - - - - - - - - - - - - - - - - - -0.119779049 0 0 0 0 0 - 31.7307 - - 0.2327 - 0 - -0.0004 - 1.0930 - 0 - 1.0344 - - - - - - - - 0.6489446 0.24638 0.17415 - - - - 0.0 0.0 1.0 1.0 - 0.0 0.0 1.0 1.0 - - - - - - - - - - - - - - - - - - - - 0.128 0 -0.142 0 0 0 - - 1 - - 0.000143971303 - 0.000140915448 - 0.000033571862 - - - - - - \ No newline at end of file From b4b5c856060edbc69a379d1cdd2a748e9590625e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 22 Nov 2021 15:58:52 +0800 Subject: [PATCH 12/43] make fast Signed-off-by: Arjo Chakravarty --- lrauv_description/test/worlds/tilted_world.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_description/test/worlds/tilted_world.sdf b/lrauv_description/test/worlds/tilted_world.sdf index a85ecadf..6bcba3b6 100644 --- a/lrauv_description/test/worlds/tilted_world.sdf +++ b/lrauv_description/test/worlds/tilted_world.sdf @@ -17,7 +17,7 @@ 0.02 - 1 + 0 Date: Mon, 22 Nov 2021 15:59:02 +0800 Subject: [PATCH 13/43] stop debug printouts Signed-off-by: Arjo Chakravarty --- lrauv_description/scripts/description_generator.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/lrauv_description/scripts/description_generator.py b/lrauv_description/scripts/description_generator.py index fc335519..e3f9bdca 100644 --- a/lrauv_description/scripts/description_generator.py +++ b/lrauv_description/scripts/description_generator.py @@ -115,9 +115,6 @@ def calculate_center_of_mass(total_mass, template_path, output_path): size = ET.SubElement(box, "size") size.text = write_float_array([2, 0.3, cube_length]) - print(total_moment + main_body_com * remaining_mass, file=sys.stderr) - print(main_body_com, file=sys.stderr) - assert sum(total_moment + main_body_com * remaining_mass) == 0 dir_name = path.dirname(output_path) From db128bd8eb0c8e924b4f8c0543cd9118cfc6841b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 22 Nov 2021 16:43:07 +0800 Subject: [PATCH 14/43] Add second test WIP Signed-off-by: Arjo Chakravarty --- .../test/test_hydrostatic_stability.cc | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index 18387432..5c732685 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -73,4 +73,63 @@ TEST(Stability, FlatWorld) } EXPECT_EQ(prev_pose.Pos(), ignition::math::Vector3d(0,0,0)); EXPECT_EQ(prev_pose.Rot(), ignition::math::Quaterniond(1,0,0,0)); +} + +TEST(Stability, TiltedWorld) +{ + auto fixture = std::make_unique( + ignition::common::joinPaths( + std::string(PROJECT_SOURCE_PATH), "test", "worlds", "tilted_world.sdf")); + + std::vector tethysPoses; + + fixture->OnPostUpdate( + [&](const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) + { + auto worldEntity = ignition::gazebo::worldEntity(_ecm); + ignition::gazebo::World world(worldEntity); + + auto modelEntity = world.ModelByName(_ecm, "tethys"); + EXPECT_NE(ignition::gazebo::kNullEntity, modelEntity); + + tethysPoses.push_back( + ignition::gazebo::worldPose(modelEntity, _ecm)); + } + ); + + fixture->Finalize(); + + fixture->Server()->Run(true, 10000, false); + EXPECT_EQ(10000, tethysPoses.size()); + + bool first = true; + + double maxPitch {std::numeric_limits::min()}, + minPitch{std::numeric_limits::max()}; + ignition::math::Pose3d prev_pose; + for (const auto &pose: tethysPoses) + { + if (first) + { + prev_pose = pose; + first = true; + continue; + } + EXPECT_NEAR(prev_pose.Pos().X(), pose.Pos().X(), 1e-2); + EXPECT_NEAR(prev_pose.Pos().Y(), pose.Pos().Y(), 1e-2); + EXPECT_NEAR(prev_pose.Pos().Z(), pose.Pos().Z(), 1e-2); + auto pitch = pose.Rot().Euler(); + if (pitch > maxPitch) + { + maxPitch = pitch; + } + + if (pitch < minPitch) + { + minPitch = pitch; + } + + prev_pose = pose; + } } \ No newline at end of file From dba0867e9b775b1a7bed572ff5cdf7e2396fade7 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 22 Nov 2021 16:44:44 +0800 Subject: [PATCH 15/43] fix compile issue Signed-off-by: Arjo Chakravarty --- lrauv_description/test/test_hydrostatic_stability.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index 5c732685..ae19215f 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -119,7 +119,7 @@ TEST(Stability, TiltedWorld) EXPECT_NEAR(prev_pose.Pos().X(), pose.Pos().X(), 1e-2); EXPECT_NEAR(prev_pose.Pos().Y(), pose.Pos().Y(), 1e-2); EXPECT_NEAR(prev_pose.Pos().Z(), pose.Pos().Z(), 1e-2); - auto pitch = pose.Rot().Euler(); + auto pitch = pose.Rot().Euler().Y(); if (pitch > maxPitch) { maxPitch = pitch; From 7748af7047a14f20d0ea04111fe64b8cd4ae2da3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 23 Nov 2021 10:36:28 +0800 Subject: [PATCH 16/43] fix CMake file Signed-off-by: Arjo Chakravarty --- lrauv_description/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_description/CMakeLists.txt b/lrauv_description/CMakeLists.txt index 46ff8ed6..5028fa20 100644 --- a/lrauv_description/CMakeLists.txt +++ b/lrauv_description/CMakeLists.txt @@ -40,7 +40,7 @@ install(FILES # Tests if(BUILD_TESTING) - find_package(ignition-gazebo6 REQUIRED COMPONENTS gui) + find_package(ignition-gazebo6 REQUIRED) set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) # Fetch and configure GTest From 4d89cdf3a2fee4c1ee0c0cc9783dacd5155a4713 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 23 Nov 2021 14:16:02 +0800 Subject: [PATCH 17/43] Add joint controllers Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf.in | 25 +++---- .../models/tethys_equipped/model.sdf | 54 +++++++------- .../scripts/description_generator.py | 71 ++++++++++++++++--- lrauv_description/test/worlds/flat_world.sdf | 11 ++- 4 files changed, 106 insertions(+), 55 deletions(-) diff --git a/lrauv_description/models/tethys/model.sdf.in b/lrauv_description/models/tethys/model.sdf.in index 0fd0cf8f..808f92ac 100644 --- a/lrauv_description/models/tethys/model.sdf.in +++ b/lrauv_description/models/tethys/model.sdf.in @@ -60,10 +60,9 @@ - + 0 0 0 0 0 0 base_link @@ -315,7 +310,7 @@ - + + - + diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf index aaed5dff..4d5c0f55 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -9,7 +9,7 @@ tethys - + - + - - + 0 1 0 - + - + 0.0244 - + 0 0 1- - + - - - + 1000 - + 0.000080 - - + 0.0005 + 0.0005 - + 0.000955 - + 0.000003 + @@ -202,7 +202,7 @@ /model/tethys/drop_weight - + diff --git a/lrauv_description/scripts/description_generator.py b/lrauv_description/scripts/description_generator.py index e3f9bdca..c0040961 100644 --- a/lrauv_description/scripts/description_generator.py +++ b/lrauv_description/scripts/description_generator.py @@ -28,11 +28,12 @@ import sys import os import os.path as path +from ignition.math import Vector3d, MassMatrix3d, Inertiald ## These are parameters of the WHOLE VEHICLE # TODO(arjo): Implement inertia. Need to implement Inertial::operator-(const Inertial&) first total_mass = 147.5671 # Total mass of the vehicle -buoyancy_z_offset = 0.007 # Buoyancy offset +buoyancy_z_offset = 0.07 # Buoyancy offset fluid_density = 1000 #fluid density def read_pose(element): @@ -45,16 +46,52 @@ def write_float_array(pose): res += str(p) + " " return res +def read_mass_matrix(tag, mass): + #TODO(arjo): Handle diagonal terms + + tag = inertia_tag.find(".inertial/inertia") + + if tag is None: + raise Exception("Inertial not defined") + + ixx, iyy, izz = (0, 0, 0) + + ixx_tag = tag.find("ixx") + if ixx_tag is not None: + ixx = float(ixx_tag.text) + + iyy_tag = tag.find("iyy") + if iyy_tag is not None: + iyy = float(iyy_tag.text) + + izz_tag = tag.find("iyy") + if izz_tag is not None: + izz = float(izz_tag.text) + + vec = Vector3d(ixx, iyy, izz) + return MassMatrix3d(mass, vec) + + +def read_inertial_tag(link): + mass_tag = link.find(".inertial/mass") + + if mass_tag is None: + raise Exception("Mass not defined") + + inertial = read_inertial_tag(link, float(mass_tag.text)) + + def calculate_center_of_mass(total_mass, template_path, output_path): sdf_file_input = ET.parse(template_path) for sdf in sdf_file_input.iter("sdf"): for model in sdf.iter("model"): - moments = [(0, np.array([0,0,0,0,0,0]))] + moments = [] main_body_com_tag = None main_body_com_pose = None collision_tag = None + skipped_masses = [] ## Get all links for links in model.iter("link"): link_pose = links.find(".pose") @@ -72,9 +109,22 @@ def calculate_center_of_mass(total_mass, template_path, output_path): if collision_element is not None: if collision_element.text.strip() == "@calculated": collision_tag = collision_element + elif collision_element.text.strip() == "@neutral_buoyancy": + collision_element.text = "" + geometry = ET.SubElement(collision_element, "geometry") + box = ET.SubElement(geometry, "box") + size = ET.SubElement(box, "size") + mass = links.find(".inertial/mass") + if mass is None: + raise Exception("Mass not found") + + sz = float(mass.text) / (0.01 * 0.01 * fluid_density) + size.text = write_float_array([0.01, 0.01, sz]) + skipped_masses.append(mass) + continue else: # TODO(arjo): Calculate moments arising from collision - pass + continue ## Calculate moments due to inertia if inertia_element is not None: @@ -90,22 +140,21 @@ def calculate_center_of_mass(total_mass, template_path, output_path): if pose is not None: center_of_mass = [float(token) for token in pose.text.split()] assert len(center_of_mass) == 6 - moments.append((float(mass.text), np.array(center_of_mass))) + moments.append((float(mass.text), np.array([center_of_mass[0]] + [0]*5))) # get the moments in the model - total_moment = sum([mass * com for mass, com in moments]) + total_moment = sum([mass * com for mass, com in moments]) # this is the mass of the various other payloads component_mass = sum([mass for mass, _ in moments]) remaining_mass = total_mass - component_mass - print(component_mass, file=sys.stderr) main_body_com = - total_moment / remaining_mass main_body_com_tag.text = str(remaining_mass) - main_body_com_pose.text = write_float_array(main_body_com) + main_body_com_pose.text = write_float_array([main_body_com[0], 0, 0, 0, 0, 0]) # calculate buoyancy position - cube_length = total_mass / (2 * 0.3 * fluid_density) + cube_length = total_mass / (2 * 0.4 * fluid_density) collision_tag.text = "" pose_tag = ET.SubElement(collision_tag, "pose") pose_tag.text = write_float_array([0, 0, buoyancy_z_offset, 0, 0, 0]) @@ -113,12 +162,12 @@ def calculate_center_of_mass(total_mass, template_path, output_path): geometry = ET.SubElement(collision_tag, "geometry") box = ET.SubElement(geometry, "box") size = ET.SubElement(box, "size") - size.text = write_float_array([2, 0.3, cube_length]) + size.text = write_float_array([2, 0.4, cube_length]) - assert sum(total_moment + main_body_com * remaining_mass) == 0 + assert (total_moment + main_body_com * remaining_mass == np.array([0]*6)).all() dir_name = path.dirname(output_path) - if not path.exists(dir_name): + if dir_name != '' and not path.exists(dir_name): os.makedirs(dir_name) sdf_file_input.write(output_path) diff --git a/lrauv_description/test/worlds/flat_world.sdf b/lrauv_description/test/worlds/flat_world.sdf index 39e458ae..deb8bf43 100644 --- a/lrauv_description/test/worlds/flat_world.sdf +++ b/lrauv_description/test/worlds/flat_world.sdf @@ -49,14 +49,21 @@ 0 0 0 0 0 0 tethys - + + + battery_joint + true + 0.0007 + From ba797f398686a02471fedfd657bf52c2b855e007 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 23 Nov 2021 14:29:00 +0800 Subject: [PATCH 18/43] Stable fins Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf.in | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/lrauv_description/models/tethys/model.sdf.in b/lrauv_description/models/tethys/model.sdf.in index 808f92ac..491d1ec0 100644 --- a/lrauv_description/models/tethys/model.sdf.in +++ b/lrauv_description/models/tethys/model.sdf.in @@ -110,7 +110,7 @@ - + + - + + --> + From 1d48f062ae1348cc789895160006870c501077a8 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 23 Nov 2021 16:07:48 +0800 Subject: [PATCH 21/43] update plugin list Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf.in | 6 +++--- lrauv_description/test/worlds/tilted_world.sdf | 15 +++++++++++++++ 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/lrauv_description/models/tethys/model.sdf.in b/lrauv_description/models/tethys/model.sdf.in index 235c071b..e39b4f01 100644 --- a/lrauv_description/models/tethys/model.sdf.in +++ b/lrauv_description/models/tethys/model.sdf.in @@ -317,11 +317,11 @@ - + 0 0 0 0 0 0 base_link propeller - + diff --git a/lrauv_description/test/worlds/tilted_world.sdf b/lrauv_description/test/worlds/tilted_world.sdf index 6bcba3b6..6d88c3d2 100644 --- a/lrauv_description/test/worlds/tilted_world.sdf +++ b/lrauv_description/test/worlds/tilted_world.sdf @@ -49,6 +49,21 @@ 0 0 0 0 0.08 0 tethys + + base_link + __model__ + drop_weight + /model/tethys/drop_weight + + + battery_joint + true + 0.0007 + From 2eb5141be2ea6329a5cb39b7daacfe8095756d67 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 23 Nov 2021 16:35:25 +0800 Subject: [PATCH 22/43] better constraints Signed-off-by: Arjo Chakravarty --- lrauv_description/test/test_hydrostatic_stability.cc | 3 +++ lrauv_description/test/worlds/tilted_world.sdf | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index ae19215f..002c4dda 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -132,4 +132,7 @@ TEST(Stability, TiltedWorld) prev_pose = pose; } + + // Since we start the system at 0.08 pitch, we should not exceed this. + EXPECT_NEAR(maxPitch, 0.08, 1e-3); } \ No newline at end of file diff --git a/lrauv_description/test/worlds/tilted_world.sdf b/lrauv_description/test/worlds/tilted_world.sdf index 6d88c3d2..cc916795 100644 --- a/lrauv_description/test/worlds/tilted_world.sdf +++ b/lrauv_description/test/worlds/tilted_world.sdf @@ -17,7 +17,7 @@ 0.02 - 0 + 1 Date: Tue, 23 Nov 2021 16:47:12 +0800 Subject: [PATCH 23/43] Add Buoyancy Engine Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys/model.sdf.in | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/lrauv_description/models/tethys/model.sdf.in b/lrauv_description/models/tethys/model.sdf.in index e39b4f01..6c52542d 100644 --- a/lrauv_description/models/tethys/model.sdf.in +++ b/lrauv_description/models/tethys/model.sdf.in @@ -243,9 +243,9 @@ - + - + 0.4 0 0 0 0 0 - + - 0.4 0 0 0 0 0 + 0.137 0 0 0 0 0 - + 0 0 0 0 0 0 base_link diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index 002c4dda..76505b24 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -100,8 +100,8 @@ TEST(Stability, TiltedWorld) fixture->Finalize(); - fixture->Server()->Run(true, 10000, false); - EXPECT_EQ(10000, tethysPoses.size()); + fixture->Server()->Run(true, 100000, false); + EXPECT_EQ(100000, tethysPoses.size()); bool first = true; @@ -134,5 +134,5 @@ TEST(Stability, TiltedWorld) } // Since we start the system at 0.08 pitch, we should not exceed this. - EXPECT_NEAR(maxPitch, 0.08, 1e-3); + EXPECT_LT(maxPitch, 0.08); } \ No newline at end of file diff --git a/lrauv_description/test/worlds/tilted_world.sdf b/lrauv_description/test/worlds/tilted_world.sdf index cc916795..6d88c3d2 100644 --- a/lrauv_description/test/worlds/tilted_world.sdf +++ b/lrauv_description/test/worlds/tilted_world.sdf @@ -17,7 +17,7 @@ 0.02 - 1 + 0 Date: Wed, 24 Nov 2021 14:16:53 +0800 Subject: [PATCH 25/43] Add newline at EOF Signed-off-by: Arjo Chakravarty --- lrauv_description/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_description/CMakeLists.txt b/lrauv_description/CMakeLists.txt index 5028fa20..380237fa 100644 --- a/lrauv_description/CMakeLists.txt +++ b/lrauv_description/CMakeLists.txt @@ -71,4 +71,4 @@ if(BUILD_TESTING) ) gtest_discover_tests(test_hydrostatic_stability) -endif() \ No newline at end of file +endif() From dce74debdc27b3073bb5c044a40eaca024b4beff Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 24 Nov 2021 23:14:54 +0800 Subject: [PATCH 26/43] Undo changes in tethys_equipped. Signed-off-by: Arjo Chakravarty --- lrauv_description/models/tethys_equipped/model.sdf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf index 0439d6e8..156ac74a 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -118,7 +118,7 @@ 0.03 0.17 0 - 0.0244--> + 0.0244 0 1 0 @@ -139,7 +139,7 @@ 0 0.0244 - 0 0 1- + 0 0 1 -1 0 0 horizontal_fins From 7eb51de8f607b323e31b35345b41e5035a7f0670 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 26 Nov 2021 10:25:30 +0800 Subject: [PATCH 27/43] Update expectation Signed-off-by: Arjo Chakravarty --- lrauv_description/scripts/description_generator.py | 8 ++++---- lrauv_description/test/test_hydrostatic_stability.cc | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/lrauv_description/scripts/description_generator.py b/lrauv_description/scripts/description_generator.py index 24c27af1..1504e803 100644 --- a/lrauv_description/scripts/description_generator.py +++ b/lrauv_description/scripts/description_generator.py @@ -32,7 +32,7 @@ ## These are parameters of the WHOLE VEHICLE # TODO(arjo): Implement inertia. Need to implement Inertial::operator-(const Inertial&) first total_mass = 147.5671 # Total mass of the vehicle -buoyancy_z_offset = 0.07 # Buoyancy offset +buoyancy_z_offset = 0.007 # Buoyancy offset fluid_density = 1000 #fluid density def read_pose(element): @@ -64,12 +64,12 @@ def calculate_center_of_mass(total_mass, template_path, output_path): center_of_mass = [0] * 6 else: # TODO(arjo) generate errors, perform protection - center_of_mass = [float(token) for token in link_pose.text.split()] + center_of_mass = [float(token) for token in link_pose.text.split()] - ## Get inertial value of part + ## Get inertial value of part inertia_element = links.find(".inertial") collision_element = links.find(".collision") - + ## Calculate buoyancy if collision_element is not None: if collision_element.text.strip() == "@calculated": diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index 76505b24..8041b1af 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -35,9 +35,9 @@ TEST(Stability, FlatWorld) auto fixture = std::make_unique( ignition::common::joinPaths( std::string(PROJECT_SOURCE_PATH), "test", "worlds", "flat_world.sdf")); - + std::vector tethysPoses; - + fixture->OnPostUpdate( [&](const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) @@ -80,9 +80,9 @@ TEST(Stability, TiltedWorld) auto fixture = std::make_unique( ignition::common::joinPaths( std::string(PROJECT_SOURCE_PATH), "test", "worlds", "tilted_world.sdf")); - + std::vector tethysPoses; - + fixture->OnPostUpdate( [&](const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) @@ -124,7 +124,7 @@ TEST(Stability, TiltedWorld) { maxPitch = pitch; } - + if (pitch < minPitch) { minPitch = pitch; @@ -134,5 +134,5 @@ TEST(Stability, TiltedWorld) } // Since we start the system at 0.08 pitch, we should not exceed this. - EXPECT_LT(maxPitch, 0.08); + EXPECT_LE(maxPitch, 0.08); } \ No newline at end of file From e81af759c65fd1dc1e00af8aa434b98fc4087837 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 26 Nov 2021 10:36:28 +0800 Subject: [PATCH 28/43] white space correction Signed-off-by: Arjo Chakravarty --- lrauv_description/test/test_hydrostatic_stability.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index 8041b1af..768e8289 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -72,7 +72,7 @@ TEST(Stability, FlatWorld) prev_pose = pose; } EXPECT_EQ(prev_pose.Pos(), ignition::math::Vector3d(0,0,0)); - EXPECT_EQ(prev_pose.Rot(), ignition::math::Quaterniond(1,0,0,0)); + EXPECT_EQ(prev_pose.Rot(), ignition::math::Quaterniond(1,0,0,0)); } TEST(Stability, TiltedWorld) From 505ea0f2d038ebe090f1d9f16cd14e5bc51b7709 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 13 Dec 2021 09:28:52 +0800 Subject: [PATCH 29/43] tag images to use recent fortress release Signed-off-by: Arjo Chakravarty --- docker/debug_integration/Dockerfile | 2 +- docker/tests/Dockerfile | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docker/debug_integration/Dockerfile b/docker/debug_integration/Dockerfile index ee30fcb6..5f6026d3 100644 --- a/docker/debug_integration/Dockerfile +++ b/docker/debug_integration/Dockerfile @@ -19,7 +19,7 @@ # Research Institute (MBARI) and the David and Lucile Packard Foundation # -FROM mbari/lrauv-ignition-sim +FROM mbari/lrauv-ignition-sim:fortress USER root diff --git a/docker/tests/Dockerfile b/docker/tests/Dockerfile index c3f3b9c2..65a0bd15 100644 --- a/docker/tests/Dockerfile +++ b/docker/tests/Dockerfile @@ -19,7 +19,7 @@ # Research Institute (MBARI) and the David and Lucile Packard Foundation # -FROM mbari/lrauv-ignition-sim +FROM mbari/lrauv-ignition-sim:fortress USER root From 4a690dcbd5eaf745a4a802387873226b36f6e161 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 14 Dec 2021 10:55:19 +0800 Subject: [PATCH 30/43] Add numpy as a dependency Signed-off-by: Arjo Chakravarty --- docker/tests/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/tests/Dockerfile b/docker/tests/Dockerfile index 65a0bd15..1689e8d3 100644 --- a/docker/tests/Dockerfile +++ b/docker/tests/Dockerfile @@ -45,7 +45,7 @@ RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable # Install the latest Ignition binaries RUN apt-get -qq update && apt-get -q -y install \ - ignition-fortress + ignition-fortress python3-numpy # Install PCL RUN apt-get update \ From 874b053acc408ec22c6d219248aaf3f80af06c21 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 14 Dec 2021 11:10:31 +0800 Subject: [PATCH 31/43] formatting Signed-off-by: Arjo Chakravarty --- lrauv_description/test/test_hydrostatic_stability.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index 768e8289..ffbd6827 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -135,4 +135,4 @@ TEST(Stability, TiltedWorld) // Since we start the system at 0.08 pitch, we should not exceed this. EXPECT_LE(maxPitch, 0.08); -} \ No newline at end of file +} From 09aa07d045207e29c0e72292b93a405dddcef14a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 14 Dec 2021 11:28:06 +0800 Subject: [PATCH 32/43] better debug Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index ae3c1632..66347a6f 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -95,7 +95,7 @@ TEST_F(LrauvTestFixture, YoYoCircle) if (i > 2000) { // Check that the vehicle actually is moving. - EXPECT_LT(0.0, linVel); + EXPECT_LT(0.0, linVel) << i; } EXPECT_NEAR(1.0, linVel, 1.0) << i; From 676259572ba1d552f71ab158f7797fa4079fe7db Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 18 Dec 2021 15:34:09 +0800 Subject: [PATCH 33/43] bump garden Signed-off-by: Arjo Chakravarty --- lrauv_description/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lrauv_description/CMakeLists.txt b/lrauv_description/CMakeLists.txt index 380237fa..4b3acc48 100644 --- a/lrauv_description/CMakeLists.txt +++ b/lrauv_description/CMakeLists.txt @@ -24,7 +24,7 @@ add_custom_command( add_custom_target(generate_model ALL DEPENDS modelsdf -) +) install(DIRECTORY models @@ -40,8 +40,8 @@ install(FILES # Tests if(BUILD_TESTING) - find_package(ignition-gazebo6 REQUIRED) - set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) + find_package(ignition-gazebo7 REQUIRED) + set(IGN_GAZEBO_VER ${ignition-gazebo7_VERSION_MAJOR}) # Fetch and configure GTest include(FetchContent) @@ -59,7 +59,7 @@ if(BUILD_TESTING) set("PROJECT_BINARY_PATH" ${CMAKE_CURRENT_BINARY_DIR}) set("PROJECT_SOURCE_PATH" ${CMAKE_CURRENT_SOURCE_DIR}) configure_file(test/helper/TestConstants.hh.in TestConstants.hh @ONLY) - + include(GoogleTest) add_executable(test_hydrostatic_stability test/test_hydrostatic_stability.cc) From dd4a7b04f566266e65fec09e5a5e691e6d543a36 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 23 Dec 2021 01:29:38 +0800 Subject: [PATCH 34/43] Update lrauv_description/test/test_hydrostatic_stability.cc Co-authored-by: Louise Poubel --- lrauv_description/test/test_hydrostatic_stability.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index ffbd6827..b991d999 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -113,7 +113,7 @@ TEST(Stability, TiltedWorld) if (first) { prev_pose = pose; - first = true; + first = false; continue; } EXPECT_NEAR(prev_pose.Pos().X(), pose.Pos().X(), 1e-2); From 602d4653146a426ee803b031480889731b91091b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 23 Dec 2021 01:30:10 +0800 Subject: [PATCH 35/43] Update lrauv_description/test/test_hydrostatic_stability.cc Co-authored-by: Louise Poubel --- lrauv_description/test/test_hydrostatic_stability.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index b991d999..61bb0e43 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -65,7 +65,7 @@ TEST(Stability, FlatWorld) if (first) { prev_pose = pose; - first = true; + first = false; continue; } EXPECT_EQ(prev_pose, pose); From 50da2a4124be5b6127583c7910b951796e4d0c67 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 3 Jan 2022 13:46:43 +0800 Subject: [PATCH 36/43] change location of python3-numpy Signed-off-by: Arjo Chakravarty --- docker/debug_integration/Dockerfile | 3 ++- docker/empty_world/Dockerfile | 1 + docker/tests/Dockerfile | 5 +++-- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/docker/debug_integration/Dockerfile b/docker/debug_integration/Dockerfile index e030016a..86041c2b 100644 --- a/docker/debug_integration/Dockerfile +++ b/docker/debug_integration/Dockerfile @@ -37,7 +37,8 @@ RUN apt-get update \ gnupg2 \ lsb-release \ tzdata \ - wget + wget \ + python3-numpy # Add Ignition's latest packages, which may be more up-to-date than the ones from the MBARI image RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \ diff --git a/docker/empty_world/Dockerfile b/docker/empty_world/Dockerfile index e18a5c41..668bfbec 100644 --- a/docker/empty_world/Dockerfile +++ b/docker/empty_world/Dockerfile @@ -48,6 +48,7 @@ RUN apt-get update \ software-properties-common \ sudo \ vim \ + python3-numpy && apt-get clean # setup timezone diff --git a/docker/tests/Dockerfile b/docker/tests/Dockerfile index b6f57103..b850d173 100644 --- a/docker/tests/Dockerfile +++ b/docker/tests/Dockerfile @@ -37,7 +37,8 @@ RUN apt-get update \ gnupg2 \ lsb-release \ tzdata \ - wget + wget \ + python3-numpy # Add Ignition's latest packages, which may be more up-to-date than the ones from the MBARI image RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \ @@ -46,7 +47,7 @@ RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable # Install the latest Ignition binaries RUN apt-get -qq update && apt-get -q -y install \ - ignition-garden python3-numpy + ignition-garden # Install PCL RUN apt-get update \ From 7bf569ba2a21ef6e76c16f5a2fa89077ba53e197 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 3 Jan 2022 13:46:51 +0800 Subject: [PATCH 37/43] rename worlds Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/worlds/acoustic_comms_test.sdf | 2 +- lrauv_ignition_plugins/worlds/star_world.sdf | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/lrauv_ignition_plugins/worlds/acoustic_comms_test.sdf b/lrauv_ignition_plugins/worlds/acoustic_comms_test.sdf index ca444284..d07d6a24 100644 --- a/lrauv_ignition_plugins/worlds/acoustic_comms_test.sdf +++ b/lrauv_ignition_plugins/worlds/acoustic_comms_test.sdf @@ -4,7 +4,7 @@ Research Institute (MBARI) and the David and Lucile Packard Foundation --> - + 0.0 1.0 1.0 diff --git a/lrauv_ignition_plugins/worlds/star_world.sdf b/lrauv_ignition_plugins/worlds/star_world.sdf index bb6b6f17..4ecf6abf 100644 --- a/lrauv_ignition_plugins/worlds/star_world.sdf +++ b/lrauv_ignition_plugins/worlds/star_world.sdf @@ -4,7 +4,7 @@ Research Institute (MBARI) and the David and Lucile Packard Foundation --> - + 0.0 1.0 1.0 From 4373b20f625200409570b43eb3e15a44a25ced27 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 3 Jan 2022 13:53:02 +0800 Subject: [PATCH 38/43] Address @mabelzhang's points Signed-off-by: Arjo Chakravarty --- lrauv_description/test/worlds/flat_world.sdf | 2 +- lrauv_description/test/worlds/tilted_world.sdf | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/lrauv_description/test/worlds/flat_world.sdf b/lrauv_description/test/worlds/flat_world.sdf index deb8bf43..4cd2f4f5 100644 --- a/lrauv_description/test/worlds/flat_world.sdf +++ b/lrauv_description/test/worlds/flat_world.sdf @@ -4,7 +4,7 @@ Research Institute (MBARI) and the David and Lucile Packard Foundation --> - + 0 0 -9.8 diff --git a/lrauv_description/test/worlds/tilted_world.sdf b/lrauv_description/test/worlds/tilted_world.sdf index 6d88c3d2..92a6f18d 100644 --- a/lrauv_description/test/worlds/tilted_world.sdf +++ b/lrauv_description/test/worlds/tilted_world.sdf @@ -4,7 +4,7 @@ Research Institute (MBARI) and the David and Lucile Packard Foundation --> - + 0 0 -9.8 From 1d0ea087e9d750934352216d0b2e3daaf8662576 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 3 Jan 2022 14:23:58 +0800 Subject: [PATCH 39/43] update hytdrostatics test Signed-off-by: Arjo Chakravarty --- .../test/test_hydrostatic_stability.cc | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/lrauv_description/test/test_hydrostatic_stability.cc b/lrauv_description/test/test_hydrostatic_stability.cc index 61bb0e43..3573eafd 100644 --- a/lrauv_description/test/test_hydrostatic_stability.cc +++ b/lrauv_description/test/test_hydrostatic_stability.cc @@ -30,8 +30,11 @@ #include #include -TEST(Stability, FlatWorld) +TEST(Stability, NeutralBuoyancy) { + /// This test checks that the vehicle is neutrally buoyant. + /// It starts the vehicle out in a non-moving fluid and expects zero + /// motion. auto fixture = std::make_unique( ignition::common::joinPaths( std::string(PROJECT_SOURCE_PATH), "test", "worlds", "flat_world.sdf")); @@ -75,8 +78,14 @@ TEST(Stability, FlatWorld) EXPECT_EQ(prev_pose.Rot(), ignition::math::Quaterniond(1,0,0,0)); } -TEST(Stability, TiltedWorld) +TEST(Stability, RestoringMoment) { + /// This test checks that the vehicle has a restoring moment. We do this by + /// starting the vehicle of in a world without hydrodynamic damping in a + /// tilted position. Since the vehicle is tilted, restoring moments should + /// kick in causing the vehicle to oscillate. In an actual setting we would + /// also have hydrodynamics which would damp the oscillations and bring the + /// boat to an equilibrium. auto fixture = std::make_unique( ignition::common::joinPaths( std::string(PROJECT_SOURCE_PATH), "test", "worlds", "tilted_world.sdf")); @@ -100,8 +109,8 @@ TEST(Stability, TiltedWorld) fixture->Finalize(); - fixture->Server()->Run(true, 100000, false); - EXPECT_EQ(100000, tethysPoses.size()); + fixture->Server()->Run(true, 10000, false); + EXPECT_EQ(10000, tethysPoses.size()); bool first = true; @@ -118,7 +127,12 @@ TEST(Stability, TiltedWorld) } EXPECT_NEAR(prev_pose.Pos().X(), pose.Pos().X(), 1e-2); EXPECT_NEAR(prev_pose.Pos().Y(), pose.Pos().Y(), 1e-2); - EXPECT_NEAR(prev_pose.Pos().Z(), pose.Pos().Z(), 1e-2); + + //Oscillations mean the Z value is not exaclty constant. + EXPECT_NEAR(prev_pose.Pos().Z(), pose.Pos().Z(), 1e-1); + + EXPECT_NEAR(prev_pose.Rot().X(), pose.Rot().X(), 1e-2); + EXPECT_NEAR(prev_pose.Rot().Z(), pose.Rot().Z(), 1e-2); auto pitch = pose.Rot().Euler().Y(); if (pitch > maxPitch) { From fb22cecd23973a450366be50be913797072c1b4b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 3 Jan 2022 14:44:53 +0800 Subject: [PATCH 40/43] Fix coilision volumes. Signed-off-by: Arjo Chakravarty --- lrauv_description/scripts/description_generator.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lrauv_description/scripts/description_generator.py b/lrauv_description/scripts/description_generator.py index 1504e803..34a1400e 100644 --- a/lrauv_description/scripts/description_generator.py +++ b/lrauv_description/scripts/description_generator.py @@ -82,9 +82,9 @@ def calculate_center_of_mass(total_mass, template_path, output_path): mass = links.find(".inertial/mass") if mass is None: raise Exception("Mass not found") - - sz = float(mass.text) / (0.01 * 0.01 * fluid_density) - size.text = write_float_array([0.01, 0.01, sz]) + + sz = float(mass.text) / (0.1 * 0.1 * fluid_density) + size.text = write_float_array([0.1, 0.1, sz]) skipped_masses.append(mass) continue else: @@ -142,5 +142,5 @@ def calculate_center_of_mass(total_mass, template_path, output_path): print("Usage:") print("description_generator.py ") exit(-100) - + calculate_center_of_mass(total_mass, sys.argv[1], sys.argv[2]) From f332e04e458556d2749338885064986ebfd0b018 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 3 Jan 2022 14:50:39 +0800 Subject: [PATCH 41/43] remove gravity. tag Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/worlds/buoyant_tethys.sdf | 1 - 1 file changed, 1 deletion(-) diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 43b3deef..748b202f 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -5,7 +5,6 @@ --> - 0 0 -9.8 0.0 1.0 1.0 From 049f65e3b205402e6d9a8d6aa84275fd71e6eff5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 3 Jan 2022 14:55:57 +0800 Subject: [PATCH 42/43] style fix Signed-off-by: Arjo Chakravarty --- lrauv_description/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/lrauv_description/CMakeLists.txt b/lrauv_description/CMakeLists.txt index 4b3acc48..f6ff5094 100644 --- a/lrauv_description/CMakeLists.txt +++ b/lrauv_description/CMakeLists.txt @@ -19,7 +19,9 @@ configure_file( # Model Generation add_custom_command( OUTPUT modelsdf - COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/scripts/description_generator.py ${CMAKE_CURRENT_SOURCE_DIR}/models/tethys/model.sdf.in ${CMAKE_CURRENT_BINARY_DIR}/models/tethys/model.sdf + COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/scripts/description_generator.py + ${CMAKE_CURRENT_SOURCE_DIR}/models/tethys/model.sdf.in + ${CMAKE_CURRENT_BINARY_DIR}/models/tethys/model.sdf ) add_custom_target(generate_model ALL From d3c21f723a5a69bf25d1c228d98193dc32a0dee1 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 5 Jan 2022 21:46:46 -0800 Subject: [PATCH 43/43] loosen tolerances, the vehicle is oscillating more Signed-off-by: Louise Poubel --- lrauv_ignition_plugins/test/test_mission_pitch_mass.cc | 8 ++++---- lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_pitch_mass.cc b/lrauv_ignition_plugins/test/test_mission_pitch_mass.cc index f65f673f..ae96aeef 100644 --- a/lrauv_ignition_plugins/test/test_mission_pitch_mass.cc +++ b/lrauv_ignition_plugins/test/test_mission_pitch_mass.cc @@ -87,16 +87,16 @@ TEST_F(LrauvTestFixture, PitchMass) { // Max Pitch 20 degrees. // Min Pitch -5 degrees. - // TODO(arjo129) Reduce tolerances - EXPECT_LT(pose.Rot().Euler().Y(), IGN_DTOR(40)); - EXPECT_GT(pose.Rot().Euler().Y(), IGN_DTOR(-5)); + // TODO(chapulina) Reduce tolerances + EXPECT_LT(pose.Rot().Euler().Y(), IGN_DTOR(50)); + EXPECT_GT(pose.Rot().Euler().Y(), IGN_DTOR(-6)); // No roll or yaw EXPECT_NEAR(pose.Rot().Euler().X(), IGN_DTOR(0), 1e-3); EXPECT_NEAR(pose.Rot().Euler().Z(), IGN_DTOR(0), 1e-3); // Check position holds - // TODO(arjo129) Reduce tolerances + // TODO(chapulina) Reduce tolerances EXPECT_NEAR(pose.Pos().X(), IGN_DTOR(0), 4.5); EXPECT_NEAR(pose.Pos().Y(), IGN_DTOR(0), 2e-1); EXPECT_NEAR(pose.Pos().Z(), IGN_DTOR(0), 2.0); diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index 6619173e..826e7875 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -93,7 +93,7 @@ TEST_F(LrauvTestFixture, YoYoCircle) // Check velocity is positive auto linVel = dist / time100it; - if (i > 2000) + if (i > 3500) { // Check that the vehicle actually is moving. EXPECT_LT(0.0, linVel) << i;