From a7b55706349d4bbd8618bc171f18b036c473c20d Mon Sep 17 00:00:00 2001 From: Andrea Ostuni Date: Sat, 23 Dec 2023 12:52:18 +0100 Subject: [PATCH 1/5] refactor ignition to gz --- scenario/CMakeLists.txt | 61 +- scenario/README.md | 4 +- scenario/bindings/CMakeLists.txt | 2 +- scenario/bindings/__init__.py | 24 +- scenario/bindings/core/CMakeLists.txt | 2 +- ...ion.cmake => FindGazeboDistribution.cmake} | 23 +- scenario/cmake/ImportTargetsCitadel.cmake | 36 +- scenario/cmake/ImportTargetsDome.cmake | 36 +- scenario/cmake/ImportTargetsEdifice.cmake | 36 +- scenario/cmake/ImportTargetsFortress.cmake | 36 +- scenario/cmake/ImportTargetsGarden.cmake | 73 +++ scenario/src/CMakeLists.txt | 4 +- scenario/src/controllers/CMakeLists.txt | 2 +- .../src/ComputedTorqueFixedBase.cpp | 12 +- scenario/src/gazebo/CMakeLists.txt | 16 +- .../include/scenario/gazebo/GazeboEntity.h | 26 +- .../include/scenario/gazebo/GazeboSimulator.h | 6 +- .../gazebo/include/scenario/gazebo/Joint.h | 14 +- .../src/gazebo/include/scenario/gazebo/Link.h | 14 +- .../src/gazebo/include/scenario/gazebo/Log.h | 12 +- .../gazebo/include/scenario/gazebo/Model.h | 14 +- .../gazebo/include/scenario/gazebo/World.h | 14 +- .../gazebo/components/BasePoseTarget.h | 28 +- .../components/BaseWorldAccelerationTarget.h | 32 +- .../components/BaseWorldVelocityTarget.h | 32 +- .../ExternalWorldWrenchCmdWithDuration.h | 26 +- .../components/HistoryOfAppliedJointForces.h | 24 +- .../gazebo/components/JointAcceleration.h | 30 +- .../components/JointAccelerationTarget.h | 26 +- .../gazebo/components/JointControlMode.h | 24 +- .../gazebo/components/JointController.h | 24 +- .../gazebo/components/JointControllerPeriod.h | 26 +- .../scenario/gazebo/components/JointPID.h | 26 +- .../gazebo/components/JointPositionTarget.h | 24 +- .../gazebo/components/JointVelocityTarget.h | 26 +- .../gazebo/components/SimulatedTime.h | 22 +- .../scenario/gazebo/components/Timestamp.h | 22 +- .../include/scenario/gazebo/exceptions.h | 18 +- .../gazebo/include/scenario/gazebo/helpers.h | 170 +++--- .../gazebo/include/scenario/gazebo/utils.h | 8 +- scenario/src/gazebo/src/GazeboSimulator.cpp | 118 ++-- scenario/src/gazebo/src/Joint.cpp | 162 ++--- scenario/src/gazebo/src/Link.cpp | 168 +++--- scenario/src/gazebo/src/Model.cpp | 270 ++++----- scenario/src/gazebo/src/World.cpp | 106 ++-- scenario/src/gazebo/src/helpers.cpp | 108 ++-- scenario/src/gazebo/src/utils.cpp | 36 +- .../plugins/ControllerRunner/CMakeLists.txt | 2 +- .../ControllerRunner/ControllerRunner.cpp | 48 +- .../ControllerRunner/ControllerRunner.h | 24 +- .../plugins/JointController/CMakeLists.txt | 2 +- .../JointController/JointController.cpp | 82 +-- .../plugins/JointController/JointController.h | 24 +- scenario/src/plugins/Physics/CMakeLists.txt | 6 +- .../Physics/CanonicalLinkModelTracker.hh | 18 +- .../src/plugins/Physics/EntityFeatureMap.hh | 22 +- scenario/src/plugins/Physics/Physics.cc | 564 +++++++++--------- scenario/src/plugins/Physics/Physics.hh | 66 +- 58 files changed, 1476 insertions(+), 1405 deletions(-) rename scenario/cmake/{FindIgnitionDistribution.cmake => FindGazeboDistribution.cmake} (84%) create mode 100644 scenario/cmake/ImportTargetsGarden.cmake diff --git a/scenario/CMakeLists.txt b/scenario/CMakeLists.txt index 0f03906c2..fd310dade 100644 --- a/scenario/CMakeLists.txt +++ b/scenario/CMakeLists.txt @@ -133,32 +133,29 @@ add_install_rpath_support( DEPENDS ENABLE_RPATH USE_LINK_PATH) -# Find a supported Ignition distribution -if(NOT IGNITION_DISTRIBUTION) +# Find a supported Gazebo distribution +if(NOT GAZEBO_DISTRIBUTION) - include(FindIgnitionDistribution) - set(SUPPORTED_IGNITION_DISTRIBUTIONS - "Fortress" - "Edifice" - "Dome" - "Citadel") + include(FindGazeboDistribution) + set(SUPPORTED_GAZEBO_DISTRIBUTIONS + "Garden") - foreach(distribution IN LISTS SUPPORTED_IGNITION_DISTRIBUTIONS) + foreach(distribution IN LISTS SUPPORTED_GAZEBO_DISTRIBUTIONS) - find_ignition_distribution( + find_gazebo_distribution( CODENAME ${distribution} PACKAGES - ignition-gazebo + gz-sim REQUIRED FALSE) if(${${distribution}_FOUND}) - message(STATUS "Found Ignition ${distribution}") + message(STATUS "Found Gazebo ${distribution}") - # Select Ignition distribution - set(IGNITION_DISTRIBUTION "${distribution}" CACHE - STRING "The Ignition distribution found in the system") - set_property(CACHE IGNITION_DISTRIBUTION PROPERTY - STRINGS ${SUPPORTED_IGNITION_DISTRIBUTIONS}) + # Select Gazebo distribution + set(GAZEBO_DISTRIBUTION "${distribution}" CACHE + STRING "The Gazebo distribution found in the system") + set_property(CACHE GAZEBO_DISTRIBUTION PROPERTY + STRINGS ${SUPPORTED_GAZEBO_DISTRIBUTIONS}) break() endif() @@ -167,24 +164,24 @@ if(NOT IGNITION_DISTRIBUTION) endif() -if(NOT IGNITION_DISTRIBUTION OR "${IGNITION_DISTRIBUTION}" STREQUAL "") - set(USE_IGNITION FALSE) +if(NOT GAZEBO_DISTRIBUTION OR "${GAZEBO_DISTRIBUTION}" STREQUAL "") + set(USE_GAZEBO FALSE) else() - set(USE_IGNITION TRUE) + set(USE_GAZEBO TRUE) endif() -option(SCENARIO_USE_IGNITION - "Build C++ code depending on Ignition" - ${USE_IGNITION}) +option(SCENARIO_USE_GAZEBO + "Build C++ code depending on Gazebo" + ${USE_GAZEBO}) -# Fail if Ignition is enabled but no compatible distribution was found -if(SCENARIO_USE_IGNITION AND "${IGNITION_DISTRIBUTION}" STREQUAL "") - message(FATAL_ERROR "Failed to find a compatible Ignition Gazebo distribution") +# Fail if Gazebo is enabled but no compatible distribution was found +if(SCENARIO_USE_GAZEBO AND "${GAZEBO_DISTRIBUTION}" STREQUAL "") + message(FATAL_ERROR "Failed to find a compatible Gazebo distribution") endif() -# Alias the Ignition targets so that we can link against different distributions -if(SCENARIO_USE_IGNITION) - include(ImportTargets${IGNITION_DISTRIBUTION}) +# Alias the Gazebo targets so that we can link against different distributions +if(SCENARIO_USE_GAZEBO) + include(ImportTargets${GAZEBO_DISTRIBUTION}) endif() # Helper for exporting targets @@ -200,9 +197,9 @@ add_subdirectory(src) # BINDINGS # ======== -# Require to find Ignition libraries when packaging for PyPI -if(SCENARIO_CALL_FROM_SETUP_PY AND NOT USE_IGNITION) - message(FATAL_ERROR "Found no Ignition distribution for PyPI package") +# Require to find Gazebo libraries when packaging for PyPI +if(SCENARIO_CALL_FROM_SETUP_PY AND NOT USE_GAZEBO) + message(FATAL_ERROR "Found no Gazebo distribution for PyPI package") endif() if(SCENARIO_ENABLE_BINDINGS) diff --git a/scenario/README.md b/scenario/README.md index 19b83062a..7eff2c944 100644 --- a/scenario/README.md +++ b/scenario/README.md @@ -41,7 +41,7 @@ These interfaces can be implemented to operate on different scenarios, including robots operating on either simulated worlds or in real-time. ScenarIO currently fully implements **Gazebo ScenarIO**, -a simulated back-end that interacts with [Ignition Gazebo](https://ignitionrobotics.org). +a simulated back-end that interacts with [Gz Sim](https://ignitionrobotics.org). The result allows stepping the simulator programmatically, ensuring a fully reproducible behaviour. It relates closely to other projects like [pybullet](https://github.com/bulletphysics/bullet3) and [mujoco-py](https://github.com/openai/mujoco-py). @@ -51,7 +51,7 @@ A real-time backend that interacts with the [YARP](https://github.com/robotology ScenarIO can be used either from C++ ([APIs](https://robotology.github.io/gym-ignition/master/breathe/core.html)) or from Python ([APIs](https://robotology.github.io/gym-ignition/master/apidoc/scenario/scenario.bindings.html)). -If you're interested to know the reasons why we started developing ScenarIO and why we selected Ignition Gazebo +If you're interested to know the reasons why we started developing ScenarIO and why we selected Gz Sim for our simulations, visit the _Motivations_ section of the [website][website]. diff --git a/scenario/bindings/CMakeLists.txt b/scenario/bindings/CMakeLists.txt index 6751b3df2..f2279571c 100644 --- a/scenario/bindings/CMakeLists.txt +++ b/scenario/bindings/CMakeLists.txt @@ -57,7 +57,7 @@ endif() # Add the SWIG folders add_subdirectory(core) -if(SCENARIO_USE_IGNITION) +if(SCENARIO_USE_GAZEBO) add_subdirectory(gazebo) endif() diff --git a/scenario/bindings/__init__.py b/scenario/bindings/__init__.py index 80ee5fbbe..4b0c34bb0 100644 --- a/scenario/bindings/__init__.py +++ b/scenario/bindings/__init__.py @@ -13,7 +13,7 @@ def supported_versions_specifier_set() -> packaging.specifiers.SpecifierSet: - # If 6 is the Ignition distribution major version, the following specifier enables + # If 6 is the Gazebo distribution major version, the following specifier enables # the compatibility with all the following versions: # # 6.Y.Z.devK @@ -55,10 +55,10 @@ def setup_gazebo_environment() -> None: import scenario.bindings.core # Configure the environment - ign_gazebo_system_plugin_path = "" + gz_sim_system_plugin_path = "" - if "IGN_GAZEBO_SYSTEM_PLUGIN_PATH" in os.environ: - ign_gazebo_system_plugin_path = os.environ.get("IGN_GAZEBO_SYSTEM_PLUGIN_PATH") + if "GZ_SIM_SYSTEM_PLUGIN_PATH" in os.environ: + gz_sim_system_plugin_path = os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH") # Exporting this env variable is done by the conda "libscenario" package if detect_install_mode() is InstallMode.CondaBuild: @@ -78,9 +78,9 @@ def setup_gazebo_environment() -> None: raise ValueError(detect_install_mode()) plugin_dir = install_prefix / "lib" / "scenario" / "plugins" - ign_gazebo_system_plugin_path += f":{str(plugin_dir)}" + gz_sim_system_plugin_path += f":{str(plugin_dir)}" - os.environ["IGN_GAZEBO_SYSTEM_PLUGIN_PATH"] = ign_gazebo_system_plugin_path + os.environ["GZ_SIM_SYSTEM_PLUGIN_PATH"] = gz_sim_system_plugin_path def preload_tensorflow_shared_libraries() -> None: @@ -137,11 +137,11 @@ def check_gazebo_installation() -> None: import subprocess try: - command = ["ign", "gazebo", "--versions"] + command = ["gz", "sim", "--versions"] result = subprocess.run(command, capture_output=True, text=True, check=True) except FileNotFoundError: - msg = "Failed to find the 'ign' command in your PATH. " - msg += "Make sure that Ignition is installed " + msg = "Failed to find the 'gz' command in your PATH. " + msg += "Make sure that Gazebo is installed " msg += "and your environment is properly configured." raise RuntimeError(msg) except subprocess.CalledProcessError: @@ -155,7 +155,7 @@ def check_gazebo_installation() -> None: # be compatible with the 'packaging' package. gazebo_version_string_normalized = gazebo_versions_string.replace("~", ".") - # The output could be multiline, listing all the Ignition Gazebo versions found + # The output could be multiline, listing all the Gz Sim versions found gazebo_versions = gazebo_version_string_normalized.split(sep=os.linesep) try: @@ -170,7 +170,7 @@ def check_gazebo_installation() -> None: if version in supported_versions_specifier_set(): return - msg = f"Failed to find Ignition Gazebo {supported_versions_specifier_set()} " + msg = f"Failed to find Gz Sim {supported_versions_specifier_set()} " msg += f"(found incompatible version(s): {gazebo_versions_parsed})" raise RuntimeError(msg) @@ -212,7 +212,7 @@ def import_gazebo() -> None: def create_home_dot_folder() -> None: # Make sure that the dot folder in the user's home exists - Path("~/.ignition/gazebo").expanduser().mkdir( + Path("~/.gz/gazebo").expanduser().mkdir( mode=0o755, parents=True, exist_ok=True ) diff --git a/scenario/bindings/core/CMakeLists.txt b/scenario/bindings/core/CMakeLists.txt index 8fd09e36b..d867b1fc2 100644 --- a/scenario/bindings/core/CMakeLists.txt +++ b/scenario/bindings/core/CMakeLists.txt @@ -28,7 +28,7 @@ set_property(TARGET ${scenario_swig_name} PROPERTY SWIG_COMPILE_OPTIONS -doxygen) # Add the to_gazebo() helpers -if(SCENARIO_USE_IGNITION) +if(SCENARIO_USE_GAZEBO) set_property(TARGET ${scenario_swig_name} PROPERTY SWIG_COMPILE_DEFINITIONS SCENARIO_HAS_GAZEBO) set_property(TARGET ${scenario_swig_name} PROPERTY diff --git a/scenario/cmake/FindIgnitionDistribution.cmake b/scenario/cmake/FindGazeboDistribution.cmake similarity index 84% rename from scenario/cmake/FindIgnitionDistribution.cmake rename to scenario/cmake/FindGazeboDistribution.cmake index 94152bc0b..17a62eb3a 100644 --- a/scenario/cmake/FindIgnitionDistribution.cmake +++ b/scenario/cmake/FindGazeboDistribution.cmake @@ -3,14 +3,15 @@ # GNU Lesser General Public License v2.1 or any later version. # In this initial implementation, we support only specifying -# the Ignition Gazebo version to determine the Ignition distribution. +# the Gz Sim version to determine the Gazebo distribution. # In the future we could pull and parse the tags.yaml file. -set(IGNITION-GAZEBO_CITADEL_VER 3) -set(IGNITION-GAZEBO_DOME_VER 4) -set(IGNITION-GAZEBO_EDIFICE_VER 5) -set(IGNITION-GAZEBO_FORTRESS_VER 6) +# set(GAZEBO_CITADEL_VER 3) +# set(GAZEBO_DOME_VER 4) +# set(GAZEBO_EDIFICE_VER 5) +# set(GAZEBO_FORTRESS_VER 6) +set(GZ-SIM_GARDEN_VER 7) -macro(find_ignition_distribution) +macro(find_gazebo_distribution) set(_prefix "fid") string(TOUPPER ${_prefix} _prefix) @@ -37,12 +38,12 @@ macro(find_ignition_distribution) # Example: # ${Citadel_FOUND}=TRUE set(${${_prefix}_CODENAME}_FOUND TRUE) - set(IGNITION_FOUND FALSE) + set(GAZEBO_FOUND FALSE) set(_pkgs_not_found) # Example: - # ${PKG}=ignition-gazebo + # ${PKG}=gz-sim foreach(PKG IN LISTS ${_prefix}_PACKAGES) # Example: @@ -62,7 +63,7 @@ macro(find_ignition_distribution) endif() # Example: - # ${_pkg_name}=ignition-gazebo3 + # ${_pkg_name}=gz-sim7 set(_pkg_name ${PKG}${${_rel_variable_name}}) # Find the package @@ -83,14 +84,14 @@ macro(find_ignition_distribution) # Print missing packages if(NOT ${${_prefix}_CODENAME}_FOUND) - message(DEBUG "Missing packages of Ignition ${${_prefix}_CODENAME}:") + message(DEBUG "Missing packages of Gazebo ${${_prefix}_CODENAME}:") foreach(PKG IN LISTS _pkgs_not_found) message(DEBUG " ${PKG}") endforeach() endif() if(${${_prefix}_CODENAME}_FOUND) - set(IGNITION_FOUND TRUE) + set(GAZEBO_FOUND TRUE) endif() unset(_prefix) diff --git a/scenario/cmake/ImportTargetsCitadel.cmake b/scenario/cmake/ImportTargetsCitadel.cmake index e1cd3e559..9c1dc7d45 100644 --- a/scenario/cmake/ImportTargetsCitadel.cmake +++ b/scenario/cmake/ImportTargetsCitadel.cmake @@ -14,60 +14,60 @@ alias_imported_target( alias_imported_target( PACKAGE_ORIG ignition-gazebo3 - PACKAGE_DEST ignition-gazebo + PACKAGE_DEST gz-sim TARGETS_ORIG ignition-gazebo3 core - TARGETS_DEST ignition-gazebo core + TARGETS_DEST gz-sim core NAMESPACE_ORIG ignition-gazebo3 - NAMESPACE_DEST ignition-gazebo + NAMESPACE_DEST gz-sim REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-common3 - PACKAGE_DEST ignition-common + PACKAGE_DEST gz-common TARGETS_ORIG ignition-common3 - TARGETS_DEST ignition-common + TARGETS_DEST gz-common NAMESPACE_ORIG ignition-common3 - NAMESPACE_DEST ignition-common + NAMESPACE_DEST gz-common REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-sensors3-all - PACKAGE_DEST ignition-sensors-all + PACKAGE_DEST gz-sensors-all TARGETS_ORIG ignition-sensors3-all - TARGETS_DEST ignition-sensors-all + TARGETS_DEST gz-sensors-all NAMESPACE_ORIG ignition-sensors3 - NAMESPACE_DEST ignition-sensors + NAMESPACE_DEST gz-sensors REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-rendering3 - PACKAGE_DEST ignition-rendering + PACKAGE_DEST gz-rendering TARGETS_ORIG ignition-rendering3 - TARGETS_DEST ignition-rendering + TARGETS_DEST gz-rendering NAMESPACE_ORIG ignition-rendering3 - NAMESPACE_DEST ignition-rendering + NAMESPACE_DEST gz-rendering REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-gazebo3-rendering - PACKAGE_DEST ignition-gazebo-rendering + PACKAGE_DEST gz-sim-rendering TARGETS_ORIG ignition-gazebo3-rendering - TARGETS_DEST ignition-gazebo-rendering + TARGETS_DEST gz-sim-rendering NAMESPACE_ORIG ignition-gazebo3 - NAMESPACE_DEST ignition-gazebo + NAMESPACE_DEST gz-sim REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-physics2 - PACKAGE_DEST ignition-physics + PACKAGE_DEST gz-physics TARGETS_ORIG ignition-physics2 - TARGETS_DEST ignition-physics + TARGETS_DEST gz-physics NAMESPACE_ORIG ignition-physics2 - NAMESPACE_DEST ignition-physics + NAMESPACE_DEST gz-physics REQUIRED TRUE ) diff --git a/scenario/cmake/ImportTargetsDome.cmake b/scenario/cmake/ImportTargetsDome.cmake index ccf3326cd..b3ad40945 100644 --- a/scenario/cmake/ImportTargetsDome.cmake +++ b/scenario/cmake/ImportTargetsDome.cmake @@ -14,61 +14,61 @@ alias_imported_target( alias_imported_target( PACKAGE_ORIG ignition-gazebo4 - PACKAGE_DEST ignition-gazebo + PACKAGE_DEST gz-sim TARGETS_ORIG ignition-gazebo4 core - TARGETS_DEST ignition-gazebo core + TARGETS_DEST gz-sim core NAMESPACE_ORIG ignition-gazebo4 - NAMESPACE_DEST ignition-gazebo + NAMESPACE_DEST gz-sim REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-common3 - PACKAGE_DEST ignition-common + PACKAGE_DEST gz-common TARGETS_ORIG ignition-common3 - TARGETS_DEST ignition-common + TARGETS_DEST gz-common NAMESPACE_ORIG ignition-common3 - NAMESPACE_DEST ignition-common + NAMESPACE_DEST gz-common REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-sensors4-all - PACKAGE_DEST ignition-sensors-all + PACKAGE_DEST gz-sensors-all TARGETS_ORIG ignition-sensors4-all - TARGETS_DEST ignition-sensors-all + TARGETS_DEST gz-sensors-all NAMESPACE_ORIG ignition-sensors4 - NAMESPACE_DEST ignition-sensors + NAMESPACE_DEST gz-sensors REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-rendering4 - PACKAGE_DEST ignition-rendering + PACKAGE_DEST gz-rendering TARGETS_ORIG ignition-rendering4 - TARGETS_DEST ignition-rendering + TARGETS_DEST gz-rendering NAMESPACE_ORIG ignition-rendering4 - NAMESPACE_DEST ignition-rendering + NAMESPACE_DEST gz-rendering REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-gazebo4-rendering - PACKAGE_DEST ignition-gazebo-rendering + PACKAGE_DEST gz-sim-rendering TARGETS_ORIG ignition-gazebo4-rendering - TARGETS_DEST ignition-gazebo-rendering + TARGETS_DEST gz-sim-rendering NAMESPACE_ORIG ignition-gazebo4 - NAMESPACE_DEST ignition-gazebo + NAMESPACE_DEST gz-sim REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-physics3 - PACKAGE_DEST ignition-physics + PACKAGE_DEST gz-physics TARGETS_ORIG ignition-physics3 - TARGETS_DEST ignition-physics + TARGETS_DEST gz-physics NAMESPACE_ORIG ignition-physics3 - NAMESPACE_DEST ignition-physics + NAMESPACE_DEST gz-physics REQUIRED TRUE ) diff --git a/scenario/cmake/ImportTargetsEdifice.cmake b/scenario/cmake/ImportTargetsEdifice.cmake index 8099dbb8c..f256a386c 100644 --- a/scenario/cmake/ImportTargetsEdifice.cmake +++ b/scenario/cmake/ImportTargetsEdifice.cmake @@ -14,60 +14,60 @@ alias_imported_target( alias_imported_target( PACKAGE_ORIG ignition-gazebo5 - PACKAGE_DEST ignition-gazebo + PACKAGE_DEST gz-sim TARGETS_ORIG ignition-gazebo5 core - TARGETS_DEST ignition-gazebo core + TARGETS_DEST gz-sim core NAMESPACE_ORIG ignition-gazebo5 - NAMESPACE_DEST ignition-gazebo + NAMESPACE_DEST gz-sim REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-common4 - PACKAGE_DEST ignition-common + PACKAGE_DEST gz-common TARGETS_ORIG ignition-common4 - TARGETS_DEST ignition-common + TARGETS_DEST gz-common NAMESPACE_ORIG ignition-common4 - NAMESPACE_DEST ignition-common + NAMESPACE_DEST gz-common REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-sensors5-all - PACKAGE_DEST ignition-sensors-all + PACKAGE_DEST gz-sensors-all TARGETS_ORIG ignition-sensors5-all - TARGETS_DEST ignition-sensors-all + TARGETS_DEST gz-sensors-all NAMESPACE_ORIG ignition-sensors5 - NAMESPACE_DEST ignition-sensors + NAMESPACE_DEST gz-sensors REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-rendering5 - PACKAGE_DEST ignition-rendering + PACKAGE_DEST gz-rendering TARGETS_ORIG ignition-rendering5 - TARGETS_DEST ignition-rendering + TARGETS_DEST gz-rendering NAMESPACE_ORIG ignition-rendering5 - NAMESPACE_DEST ignition-rendering + NAMESPACE_DEST gz-rendering REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-gazebo5-rendering - PACKAGE_DEST ignition-gazebo-rendering + PACKAGE_DEST gz-sim-rendering TARGETS_ORIG ignition-gazebo5-rendering - TARGETS_DEST ignition-gazebo-rendering + TARGETS_DEST gz-sim-rendering NAMESPACE_ORIG ignition-gazebo5 - NAMESPACE_DEST ignition-gazebo + NAMESPACE_DEST gz-sim REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-physics4 - PACKAGE_DEST ignition-physics + PACKAGE_DEST gz-physics TARGETS_ORIG ignition-physics4 - TARGETS_DEST ignition-physics + TARGETS_DEST gz-physics NAMESPACE_ORIG ignition-physics4 - NAMESPACE_DEST ignition-physics + NAMESPACE_DEST gz-physics REQUIRED TRUE ) diff --git a/scenario/cmake/ImportTargetsFortress.cmake b/scenario/cmake/ImportTargetsFortress.cmake index 57f7575cd..e69fb4e12 100644 --- a/scenario/cmake/ImportTargetsFortress.cmake +++ b/scenario/cmake/ImportTargetsFortress.cmake @@ -14,60 +14,60 @@ alias_imported_target( alias_imported_target( PACKAGE_ORIG ignition-gazebo6 - PACKAGE_DEST ignition-gazebo + PACKAGE_DEST gz-sim TARGETS_ORIG ignition-gazebo6 core - TARGETS_DEST ignition-gazebo core + TARGETS_DEST gz-sim core NAMESPACE_ORIG ignition-gazebo6 - NAMESPACE_DEST ignition-gazebo + NAMESPACE_DEST gz-sim REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-common4 - PACKAGE_DEST ignition-common + PACKAGE_DEST gz-common TARGETS_ORIG ignition-common4 - TARGETS_DEST ignition-common + TARGETS_DEST gz-common NAMESPACE_ORIG ignition-common4 - NAMESPACE_DEST ignition-common + NAMESPACE_DEST gz-common REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-sensors6-all - PACKAGE_DEST ignition-sensors-all + PACKAGE_DEST gz-sensors-all TARGETS_ORIG ignition-sensors6-all - TARGETS_DEST ignition-sensors-all + TARGETS_DEST gz-sensors-all NAMESPACE_ORIG ignition-sensors6 - NAMESPACE_DEST ignition-sensors + NAMESPACE_DEST gz-sensors REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-rendering6 - PACKAGE_DEST ignition-rendering + PACKAGE_DEST gz-rendering TARGETS_ORIG ignition-rendering6 - TARGETS_DEST ignition-rendering + TARGETS_DEST gz-rendering NAMESPACE_ORIG ignition-rendering6 - NAMESPACE_DEST ignition-rendering + NAMESPACE_DEST gz-rendering REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-gazebo6-rendering - PACKAGE_DEST ignition-gazebo-rendering + PACKAGE_DEST gz-sim-rendering TARGETS_ORIG ignition-gazebo6-rendering - TARGETS_DEST ignition-gazebo-rendering + TARGETS_DEST gz-sim-rendering NAMESPACE_ORIG ignition-gazebo6 - NAMESPACE_DEST ignition-gazebo + NAMESPACE_DEST gz-sim REQUIRED TRUE ) alias_imported_target( PACKAGE_ORIG ignition-physics5 - PACKAGE_DEST ignition-physics + PACKAGE_DEST gz-physics TARGETS_ORIG ignition-physics5 - TARGETS_DEST ignition-physics + TARGETS_DEST gz-physics NAMESPACE_ORIG ignition-physics5 - NAMESPACE_DEST ignition-physics + NAMESPACE_DEST gz-physics REQUIRED TRUE ) diff --git a/scenario/cmake/ImportTargetsGarden.cmake b/scenario/cmake/ImportTargetsGarden.cmake new file mode 100644 index 000000000..27e85db8f --- /dev/null +++ b/scenario/cmake/ImportTargetsGarden.cmake @@ -0,0 +1,73 @@ +include(AliasImportedTarget) + +# https://ignitionrobotics.org/docs/fortress/install#fortress-libraries + +alias_imported_target( + PACKAGE_ORIG sdformat13 + PACKAGE_DEST sdformat + TARGETS_ORIG sdformat13 + TARGETS_DEST sdformat + NAMESPACE_ORIG sdformat13 + NAMESPACE_DEST sdformat + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG gz-sim7 + PACKAGE_DEST gz-sim + TARGETS_ORIG gz-sim7 core + TARGETS_DEST gz-sim core + NAMESPACE_ORIG gz-sim7 + NAMESPACE_DEST gz-sim + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG gz-common5 + PACKAGE_DEST gz-common + TARGETS_ORIG gz-common5 + TARGETS_DEST gz-common + NAMESPACE_ORIG gz-common5 + NAMESPACE_DEST gz-common + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG gz-sensors7-all + PACKAGE_DEST gz-sensors-all + TARGETS_ORIG gz-sensors7-all + TARGETS_DEST gz-sensors-all + NAMESPACE_ORIG gz-sensors7 + NAMESPACE_DEST gz-sensors7 + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG gz-rendering7 + PACKAGE_DEST gz-rendering + TARGETS_ORIG gz-rendering7 + TARGETS_DEST gz-rendering + NAMESPACE_ORIG gz-rendering7 + NAMESPACE_DEST gz-rendering + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG gz-sim7-rendering + PACKAGE_DEST gz-sim-rendering + TARGETS_ORIG gz-sim7-rendering + TARGETS_DEST gz-sim-rendering + NAMESPACE_ORIG gz-sim7 + NAMESPACE_DEST gz-sim + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG gz-physics6 + PACKAGE_DEST gz-physics + TARGETS_ORIG gz-physics6 + TARGETS_DEST gz-physics + NAMESPACE_ORIG gz-physics6 + NAMESPACE_DEST gz-physics + REQUIRED TRUE + ) diff --git a/scenario/src/CMakeLists.txt b/scenario/src/CMakeLists.txt index 7721c9f77..6a3d8b549 100644 --- a/scenario/src/CMakeLists.txt +++ b/scenario/src/CMakeLists.txt @@ -7,9 +7,9 @@ add_subdirectory(core) set(SCENARIO_COMPONENTS ScenarioCore) set(SCENARIO_PRIVATE_DEPENDENCIES "") -if(SCENARIO_USE_IGNITION) +if(SCENARIO_USE_GAZEBO) - option(ENABLE_PROFILER "Enable Ignition Profiler" OFF) + option(ENABLE_PROFILER "Enable Gazebo Profiler" OFF) mark_as_advanced(ENABLE_PROFILER) add_subdirectory(gazebo) diff --git a/scenario/src/controllers/CMakeLists.txt b/scenario/src/controllers/CMakeLists.txt index a809a81f2..4d5199a0b 100644 --- a/scenario/src/controllers/CMakeLists.txt +++ b/scenario/src/controllers/CMakeLists.txt @@ -46,7 +46,7 @@ target_link_libraries(ComputedTorqueFixedBase ScenarioCore::ScenarioABC iDynTree::idyntree-core iDynTree::idyntree-model - iDynTree::idyntree-modelio-urdf + iDynTree::idyntree-modelio iDynTree::idyntree-high-level) target_include_directories(ComputedTorqueFixedBase PUBLIC diff --git a/scenario/src/controllers/src/ComputedTorqueFixedBase.cpp b/scenario/src/controllers/src/ComputedTorqueFixedBase.cpp index 201b54b4c..02cf89790 100644 --- a/scenario/src/controllers/src/ComputedTorqueFixedBase.cpp +++ b/scenario/src/controllers/src/ComputedTorqueFixedBase.cpp @@ -13,13 +13,13 @@ #include "scenario/core/utils/Log.h" #include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include -#include +#include +#include #include #include diff --git a/scenario/src/gazebo/CMakeLists.txt b/scenario/src/gazebo/CMakeLists.txt index 7d3f54fe9..259590aa8 100644 --- a/scenario/src/gazebo/CMakeLists.txt +++ b/scenario/src/gazebo/CMakeLists.txt @@ -51,7 +51,7 @@ target_include_directories(ExtraComponents INTERFACE $ $) -target_link_libraries(ExtraComponents INTERFACE ${ignition-gazebo.core}) +target_link_libraries(ExtraComponents INTERFACE ${gz-sim.core}) set_target_properties(ExtraComponents PROPERTIES PUBLIC_HEADER "${EXTRA_COMPONENTS_PUBLIC_HEADERS}") @@ -94,13 +94,13 @@ target_include_directories(ScenarioGazebo PUBLIC target_link_libraries(ScenarioGazebo PUBLIC ScenarioCore::ScenarioABC - ${ignition-gazebo.core} - ${ignition-common.ignition-common} + ${gz-sim.core} + ${gz-common.gz-common} PRIVATE ScenarioCore::CoreUtils ScenarioGazebo::ExtraComponents - ${ignition-physics.ignition-physics} - ${ignition-fuel_tools.ignition-fuel_tools}) + ${gz-physics.gz-physics} + ${gz-fuel_tools.gz-fuel_tools}) set_target_properties(ScenarioGazebo PROPERTIES PUBLIC_HEADER "${SCENARIO_GAZEBO_PUBLIC_HDRS}") @@ -123,11 +123,11 @@ target_link_libraries(GazeboSimulator ScenarioCore::ScenarioABC PRIVATE tiny-process-library::tiny-process-library - ${ignition-gazebo.core} + ${gz-sim.core} ScenarioCore::CoreUtils ScenarioGazebo::ScenarioGazebo ScenarioGazebo::ExtraComponents - ${ignition-fuel_tools.ignition-fuel_tools}) + ${gz-fuel_tools.gz-fuel_tools}) set_target_properties(GazeboSimulator PROPERTIES PUBLIC_HEADER include/scenario/gazebo/GazeboSimulator.h) @@ -171,7 +171,7 @@ install_basic_package_files(ScenarioGazebo VERSION ${PROJECT_VERSION} COMPATIBILITY AnyNewerVersion EXPORT ScenarioGazeboExport - DEPENDENCIES ScenarioCore ${ignition-gazebo} ${ignition-common} + DEPENDENCIES ScenarioCore ${gz-sim} ${gz-common} NAMESPACE ScenarioGazebo:: NO_CHECK_REQUIRED_COMPONENTS_MACRO INSTALL_DESTINATION diff --git a/scenario/src/gazebo/include/scenario/gazebo/GazeboEntity.h b/scenario/src/gazebo/include/scenario/gazebo/GazeboEntity.h index 7d8224fae..2a9417376 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/GazeboEntity.h +++ b/scenario/src/gazebo/include/scenario/gazebo/GazeboEntity.h @@ -27,9 +27,9 @@ #ifndef SCENARIO_GAZEBO_GAZEBOENTITY_H #define SCENARIO_GAZEBO_GAZEBOENTITY_H -#include -#include -#include +#include +#include +#include #include @@ -62,9 +62,9 @@ class scenario::gazebo::GazeboEntity * @param eventManager The pointer to the EventManager. * @return True for success, false otherwise. */ - virtual bool initialize(const ignition::gazebo::Entity linkEntity, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventManager) = 0; + virtual bool initialize(const gz::sim::Entity linkEntity, + gz::sim::EntityComponentManager* ecm, + gz::sim::EventManager* eventManager) = 0; /** * Initialize the object. @@ -80,14 +80,14 @@ class scenario::gazebo::GazeboEntity * * @return The entity that corresponds to this object. */ - inline ignition::gazebo::Entity entity() const { return this->m_entity; } + inline gz::sim::Entity entity() const { return this->m_entity; } /** * Return the pointer to the event manager. * * @return The pointer to the event manager. */ - inline ignition::gazebo::EventManager* eventManager() const + inline gz::sim::EventManager* eventManager() const { return this->m_eventManager; } @@ -97,7 +97,7 @@ class scenario::gazebo::GazeboEntity * * @return The pointer to the Entity Component Manager. */ - inline ignition::gazebo::EntityComponentManager* ecm() const + inline gz::sim::EntityComponentManager* ecm() const { return this->m_ecm; } @@ -110,13 +110,13 @@ class scenario::gazebo::GazeboEntity inline bool validEntity() const { return m_eventManager && m_ecm - && m_entity != ignition::gazebo::kNullEntity; + && m_entity != gz::sim::kNullEntity; } protected: - ignition::gazebo::EventManager* m_eventManager = nullptr; - ignition::gazebo::EntityComponentManager* m_ecm = nullptr; - ignition::gazebo::Entity m_entity = ignition::gazebo::kNullEntity; + gz::sim::EventManager* m_eventManager = nullptr; + gz::sim::EntityComponentManager* m_ecm = nullptr; + gz::sim::Entity m_entity = gz::sim::kNullEntity; }; #endif // SCENARIO_GAZEBO_GAZEBOENTITY_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/GazeboSimulator.h b/scenario/src/gazebo/include/scenario/gazebo/GazeboSimulator.h index 8a545eac6..e3fd39362 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/GazeboSimulator.h +++ b/scenario/src/gazebo/include/scenario/gazebo/GazeboSimulator.h @@ -42,7 +42,7 @@ class scenario::gazebo::GazeboSimulator { public: /** - * Class wrapping the Ignition Gazebo simulator. + * Class wrapping the Gz Sim simulator. * * The simulator can execute either unpaused or paused runs. Paused runs * update the internal state of the simulator without stepping the physics. @@ -105,7 +105,7 @@ class scenario::gazebo::GazeboSimulator bool run(const bool paused = false); /** - * Open the Ignition Gazebo GUI. + * Open the Gz Sim GUI. * * @param verbosity Configure the verbosity of the GUI (0-4) * @return True for success, false otherwise. @@ -166,7 +166,7 @@ class scenario::gazebo::GazeboSimulator * @return True for success, false otherwise. * * @warning This is an experimental feature. Multiworld simulations are only - * partially supported by Ignition Gazebo. + * partially supported by Gz Sim. */ bool insertWorldsFromSDF(const std::string& worldFile, const std::vector& worldNames = {}); diff --git a/scenario/src/gazebo/include/scenario/gazebo/Joint.h b/scenario/src/gazebo/include/scenario/gazebo/Joint.h index ad1c98df6..9e8c0cbac 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/Joint.h +++ b/scenario/src/gazebo/include/scenario/gazebo/Joint.h @@ -30,9 +30,9 @@ #include "scenario/core/Joint.h" #include "scenario/gazebo/GazeboEntity.h" -#include -#include -#include +#include +#include +#include #include #include @@ -57,9 +57,9 @@ class scenario::gazebo::Joint final uint64_t id() const override; - bool initialize(const ignition::gazebo::Entity jointEntity, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventManager) override; + bool initialize(const gz::sim::Entity jointEntity, + gz::sim::EntityComponentManager* ecm, + gz::sim::EventManager* eventManager) override; bool createECMResources() override; @@ -68,7 +68,7 @@ class scenario::gazebo::Joint final // ============ /** - * Insert a Ignition Gazebo plugin to the joint. + * Insert a Gz Gazebo plugin to the joint. * * @param libName The library name of the plugin. * @param className The class name (or alias) of the plugin. diff --git a/scenario/src/gazebo/include/scenario/gazebo/Link.h b/scenario/src/gazebo/include/scenario/gazebo/Link.h index 639a11c62..851b91069 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/Link.h +++ b/scenario/src/gazebo/include/scenario/gazebo/Link.h @@ -30,9 +30,9 @@ #include "scenario/core/Link.h" #include "scenario/gazebo/GazeboEntity.h" -#include -#include -#include +#include +#include +#include #include #include @@ -59,9 +59,9 @@ class scenario::gazebo::Link final uint64_t id() const override; - bool initialize(const ignition::gazebo::Entity linkEntity, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventManager) override; + bool initialize(const gz::sim::Entity linkEntity, + gz::sim::EntityComponentManager* ecm, + gz::sim::EventManager* eventManager) override; bool createECMResources() override; @@ -70,7 +70,7 @@ class scenario::gazebo::Link final // =========== /** - * Insert a Ignition Gazebo plugin to the link. + * Insert a Gz Gazebo plugin to the link. * * @param libName The library name of the plugin. * @param className The class name (or alias) of the plugin. diff --git a/scenario/src/gazebo/include/scenario/gazebo/Log.h b/scenario/src/gazebo/include/scenario/gazebo/Log.h index 50a398dfb..97ab2998a 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/Log.h +++ b/scenario/src/gazebo/include/scenario/gazebo/Log.h @@ -30,12 +30,12 @@ #ifndef SCENARIO_LOG_MACROS_DEFINED #define SCENARIO_LOG_MACROS_DEFINED -#include -#define sError ::ignition::common::Console::err(__FILE__, __LINE__) -#define sWarning ::ignition::common::Console::warn(__FILE__, __LINE__) -#define sMessage ::ignition::common::Console::msg(__FILE__, __LINE__) -#define sDebug ::ignition::common::Console::dbg(__FILE__, __LINE__) -#define sLog ::ignition::common::Console::log(__FILE__, __LINE__) +#include +#define sError ::gz::common::Console::err(__FILE__, __LINE__) +#define sWarning ::gz::common::Console::warn(__FILE__, __LINE__) +#define sMessage ::gz::common::Console::msg(__FILE__, __LINE__) +#define sDebug ::gz::common::Console::dbg(__FILE__, __LINE__) +#define sLog ::gz::common::Console::log(__FILE__, __LINE__) #endif // SCENARIO_LOG_MACROS_DEFINED diff --git a/scenario/src/gazebo/include/scenario/gazebo/Model.h b/scenario/src/gazebo/include/scenario/gazebo/Model.h index a45adfc1d..b0c7cf47b 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/Model.h +++ b/scenario/src/gazebo/include/scenario/gazebo/Model.h @@ -30,9 +30,9 @@ #include "scenario/core/Model.h" #include "scenario/gazebo/GazeboEntity.h" -#include -#include -#include +#include +#include +#include #include #include @@ -58,9 +58,9 @@ class scenario::gazebo::Model final uint64_t id() const override; - bool initialize(const ignition::gazebo::Entity modelEntity, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventManager) override; + bool initialize(const gz::sim::Entity modelEntity, + gz::sim::EntityComponentManager* ecm, + gz::sim::EventManager* eventManager) override; bool createECMResources() override; // ============ @@ -68,7 +68,7 @@ class scenario::gazebo::Model final // ============ /** - * Insert a Ignition Gazebo plugin to the model. + * Insert a Gz Gazebo plugin to the model. * * @param libName The library name of the plugin. * @param className The class name (or alias) of the plugin. diff --git a/scenario/src/gazebo/include/scenario/gazebo/World.h b/scenario/src/gazebo/include/scenario/gazebo/World.h index 5bec422db..2a4c31f4b 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/World.h +++ b/scenario/src/gazebo/include/scenario/gazebo/World.h @@ -30,9 +30,9 @@ #include "scenario/core/World.h" #include "scenario/gazebo/GazeboEntity.h" -#include -#include -#include +#include +#include +#include #include #include @@ -68,9 +68,9 @@ class scenario::gazebo::World final uint64_t id() const override; - bool initialize(const ignition::gazebo::Entity worldEntity, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventManager) override; + bool initialize(const gz::sim::Entity worldEntity, + gz::sim::EntityComponentManager* ecm, + gz::sim::EventManager* eventManager) override; bool createECMResources() override; @@ -79,7 +79,7 @@ class scenario::gazebo::World final // ============ /** - * Insert a Ignition Gazebo plugin to the world. + * Insert a Gz Gazebo plugin to the world. * * @param libName The library name of the plugin. * @param className The class name (or alias) of the plugin. diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/BasePoseTarget.h b/scenario/src/gazebo/include/scenario/gazebo/components/BasePoseTarget.h index 8b6f62224..3e4d1ade8 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/BasePoseTarget.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/BasePoseTarget.h @@ -24,26 +24,26 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_BASEPOSETARGET_H -#define IGNITION_GAZEBO_COMPONENTS_BASEPOSETARGET_H +#ifndef GZ_SIM_COMPONENTS_BASEPOSETARGET_H +#define GZ_SIM_COMPONENTS_BASEPOSETARGET_H -#include -#include -#include -#include +#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Base pose target used by floating base controllers. using BasePoseTarget = - Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.BasePoseTarget", + Component; + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.BasePoseTarget", BasePoseTarget) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_BASEPOSETARGET_H +#endif // GZ_SIM_COMPONENTS_BASEPOSETARGET_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/BaseWorldAccelerationTarget.h b/scenario/src/gazebo/include/scenario/gazebo/components/BaseWorldAccelerationTarget.h index cf07dc6ce..5188b962d 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/BaseWorldAccelerationTarget.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/BaseWorldAccelerationTarget.h @@ -24,26 +24,26 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_BASEWORLDACCELERATIONTARGET_H -#define IGNITION_GAZEBO_COMPONENTS_BASEWORLDACCELERATIONTARGET_H +#ifndef GZ_SIM_COMPONENTS_BASEWORLDACCELERATIONTARGET_H +#define GZ_SIM_COMPONENTS_BASEWORLDACCELERATIONTARGET_H -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Base world linear acceleration target used by /// floating base controllers. using BaseWorldLinearAccelerationTarget = Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.BaseWorldLinearAccelerationTarget", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.BaseWorldLinearAccelerationTarget", BaseWorldLinearAccelerationTarget) /// \brief Base world angular acceleration target used by @@ -51,12 +51,12 @@ namespace ignition::gazebo { using BaseWorldAngularAccelerationTarget = Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components." + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components." "BaseWorldAngularAccelerationTargetTarget", BaseWorldAngularAccelerationTarget) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_BASEWORLDACCELERATIONTARGET_H +#endif // GZ_SIM_COMPONENTS_BASEWORLDACCELERATIONTARGET_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/BaseWorldVelocityTarget.h b/scenario/src/gazebo/include/scenario/gazebo/components/BaseWorldVelocityTarget.h index 516ab26df..3f15d6896 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/BaseWorldVelocityTarget.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/BaseWorldVelocityTarget.h @@ -24,26 +24,26 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_BASEWORLDVELOCITYTARGET_H -#define IGNITION_GAZEBO_COMPONENTS_BASEWORLDVELOCITYTARGET_H +#ifndef GZ_SIM_COMPONENTS_BASEWORLDVELOCITYTARGET_H +#define GZ_SIM_COMPONENTS_BASEWORLDVELOCITYTARGET_H -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Base world linear velocity target used by /// floating base controllers. using BaseWorldLinearVelocityTarget = Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.BaseWorldLinearVelocityTarget", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.BaseWorldLinearVelocityTarget", BaseWorldLinearVelocityTarget) /// \brief Base world angular velocity target used by @@ -51,12 +51,12 @@ namespace ignition::gazebo { using BaseWorldAngularVelocityTarget = Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components." + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components." "BaseWorldAngularVelocityTargetTarget", BaseWorldAngularVelocityTarget) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_BASEWORLDVELOCITYTARGET_H +#endif // GZ_SIM_COMPONENTS_BASEWORLDVELOCITYTARGET_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/ExternalWorldWrenchCmdWithDuration.h b/scenario/src/gazebo/include/scenario/gazebo/components/ExternalWorldWrenchCmdWithDuration.h index 4d976b6d5..a8eff65d4 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/ExternalWorldWrenchCmdWithDuration.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/ExternalWorldWrenchCmdWithDuration.h @@ -24,25 +24,25 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_EXTERNALWORLDWRENCHCMDWITHDURATION_H -#define IGNITION_GAZEBO_COMPONENTS_EXTERNALWORLDWRENCHCMDWITHDURATION_H +#ifndef GZ_SIM_COMPONENTS_EXTERNALWORLDWRENCHCMDWITHDURATION_H +#define GZ_SIM_COMPONENTS_EXTERNALWORLDWRENCHCMDWITHDURATION_H #include "scenario/gazebo/helpers.h" -#include -#include -#include +#include +#include +#include #include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief A component type that contains the external wrench to be /// applied for a given duration on an entity expressed in /// the world frame and represented by - /// ignition::msgs::Wrench. Currently this is used for + /// gz::msgs::Wrench. Currently this is used for /// applying wrenches on links. Although the msg::Wrench type /// has a force_offset member, the value is currently /// ignored. Instead, the force is applied at the link @@ -51,11 +51,11 @@ namespace ignition::gazebo { using ExternalWorldWrenchCmdWithDuration = Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.ExternalWorldWrenchCmdWithDuration", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.ExternalWorldWrenchCmdWithDuration", ExternalWorldWrenchCmdWithDuration) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_EXTERNALWORLDWRENCHCMDWITHDURATION_H +#endif // GZ_SIM_COMPONENTS_EXTERNALWORLDWRENCHCMDWITHDURATION_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/HistoryOfAppliedJointForces.h b/scenario/src/gazebo/include/scenario/gazebo/components/HistoryOfAppliedJointForces.h index d844b2920..e490f66ab 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/HistoryOfAppliedJointForces.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/HistoryOfAppliedJointForces.h @@ -24,18 +24,18 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_HISTORYOFAPPLIEDJOINTFORCES_H -#define IGNITION_GAZEBO_COMPONENTS_HISTORYOFAPPLIEDJOINTFORCES_H +#ifndef GZ_SIM_COMPONENTS_HISTORYOFAPPLIEDJOINTFORCES_H +#define GZ_SIM_COMPONENTS_HISTORYOFAPPLIEDJOINTFORCES_H #include "scenario/gazebo/helpers.h" -#include -#include -#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Fixed-size queue that stores a window of applied joint /// forces. @@ -45,11 +45,11 @@ namespace ignition::gazebo { using HistoryOfAppliedJointForces = Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.HistoryOfAppliedJointForces", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.HistoryOfAppliedJointForces", HistoryOfAppliedJointForces) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_HISTORYOFAPPLIEDJOINTFORCES_H +#endif // GZ_SIM_COMPONENTS_HISTORYOFAPPLIEDJOINTFORCES_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/JointAcceleration.h b/scenario/src/gazebo/include/scenario/gazebo/components/JointAcceleration.h index bd7c294ee..324a52a6b 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/JointAcceleration.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/JointAcceleration.h @@ -24,20 +24,20 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTACCELERATION_H -#define IGNITION_GAZEBO_COMPONENTS_JOINTACCELERATION_H +#ifndef GZ_SIM_COMPONENTS_JOINTACCELERATION_H +#define GZ_SIM_COMPONENTS_JOINTACCELERATION_H #include -#include -#include -#include -#include +#include +#include +#include +#include -namespace ignition { - namespace gazebo { +namespace gz { + namespace sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Joint acceleration in SI units (rad/s/s for revolute, /// m/s/s for prismatic). The component wraps a std::vector of @@ -46,12 +46,12 @@ namespace ignition { Component, class JointAccelerationTag, serializers::VectorDoubleSerializer>; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.JointAcceleration", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.JointAcceleration", JointAcceleration) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE - } // namespace gazebo -} // namespace ignition + } // namespace GZ_SIM_VERSION_NAMESPACE + } // namespace sim +} // namespace gz -#endif // IGNITION_GAZEBO_COMPONENTS_JOINTACCELERATION_H +#endif // GZ_SIM_COMPONENTS_JOINTACCELERATION_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/JointAccelerationTarget.h b/scenario/src/gazebo/include/scenario/gazebo/components/JointAccelerationTarget.h index ff58258a3..45194f387 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/JointAccelerationTarget.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/JointAccelerationTarget.h @@ -24,19 +24,19 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTACCELERATIONTARGET_H -#define IGNITION_GAZEBO_COMPONENTS_JOINTACCELERATIONTARGET_H +#ifndef GZ_SIM_COMPONENTS_JOINTACCELERATIONTARGET_H +#define GZ_SIM_COMPONENTS_JOINTACCELERATIONTARGET_H #include -#include -#include -#include -#include +#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Joint acceleration target in SI units (rad/s/s for /// revolute, m/s/s for prismatic) used by joint @@ -48,11 +48,11 @@ namespace ignition::gazebo { Component, class JointAccelerationTargetTag, serializers::VectorDoubleSerializer>; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.JointAccelerationTarget", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.JointAccelerationTarget", JointAccelerationTarget) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_JOINTACCELERATIONTARGET_H +#endif // GZ_SIM_COMPONENTS_JOINTACCELERATIONTARGET_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/JointControlMode.h b/scenario/src/gazebo/include/scenario/gazebo/components/JointControlMode.h index edecd499d..ed1408863 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/JointControlMode.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/JointControlMode.h @@ -24,18 +24,18 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLMODE_H -#define IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLMODE_H +#ifndef GZ_SIM_COMPONENTS_JOINTCONTROLMODE_H +#define GZ_SIM_COMPONENTS_JOINTCONTROLMODE_H #include "scenario/gazebo/Joint.h" -#include -#include -#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace serializers { class JointControlModeSerializer; } // namespace serializers @@ -44,11 +44,11 @@ namespace ignition::gazebo { /// \brief Joint control mode. using JointControlMode = Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.JointControlMode", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.JointControlMode", JointControlMode) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLMODE_H +#endif // GZ_SIM_COMPONENTS_JOINTCONTROLMODE_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/JointController.h b/scenario/src/gazebo/include/scenario/gazebo/components/JointController.h index 0101423fa..7f7aa5c40 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/JointController.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/JointController.h @@ -24,24 +24,24 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLLER_H -#define IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLLER_H +#ifndef GZ_SIM_COMPONENTS_JOINTCONTROLLER_H +#define GZ_SIM_COMPONENTS_JOINTCONTROLLER_H -#include -#include -#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Marks whether a model has a JointController plugin. using JointController = Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.JointController", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.JointController", JointController) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLLER_H +#endif // GZ_SIM_COMPONENTS_JOINTCONTROLLER_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/JointControllerPeriod.h b/scenario/src/gazebo/include/scenario/gazebo/components/JointControllerPeriod.h index ab033cf8d..e9d88005b 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/JointControllerPeriod.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/JointControllerPeriod.h @@ -24,29 +24,29 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLLERPERIOD_H -#define IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLLERPERIOD_H +#ifndef GZ_SIM_COMPONENTS_JOINTCONTROLLERPERIOD_H +#define GZ_SIM_COMPONENTS_JOINTCONTROLLERPERIOD_H -#include -#include -#include -#include +#include +#include +#include +#include #include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Joint controller period in seconds. using JointControllerPeriod = Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.JointControllerPeriod", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.JointControllerPeriod", JointControllerPeriod) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLLERPERIOD_H +#endif // GZ_SIM_COMPONENTS_JOINTCONTROLLERPERIOD_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/JointPID.h b/scenario/src/gazebo/include/scenario/gazebo/components/JointPID.h index b8a1fdd72..8e8be2559 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/JointPID.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/JointPID.h @@ -24,24 +24,24 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTPID_H -#define IGNITION_GAZEBO_COMPONENTS_JOINTPID_H +#ifndef GZ_SIM_COMPONENTS_JOINTPID_H +#define GZ_SIM_COMPONENTS_JOINTPID_H -#include -#include -#include -#include +#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Joint PID controller. - using JointPID = Component; - IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointPID", + using JointPID = Component; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.JointPID", JointPID) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_JOINTPID_H +#endif // GZ_SIM_COMPONENTS_JOINTPID_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/JointPositionTarget.h b/scenario/src/gazebo/include/scenario/gazebo/components/JointPositionTarget.h index a9306ceff..698f5ddc9 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/JointPositionTarget.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/JointPositionTarget.h @@ -24,18 +24,18 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONTARGET_H -#define IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONTARGET_H +#ifndef GZ_SIM_COMPONENTS_JOINTPOSITIONTARGET_H +#define GZ_SIM_COMPONENTS_JOINTPOSITIONTARGET_H #include -#include -#include -#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Joint position target in SI units (rad for revolute, /// m for prismatic) used by joint controllers. @@ -44,11 +44,11 @@ namespace ignition::gazebo { /// degrees of freedom of the joint. using JointPositionTarget = Component, class JointPositionTargetTag>; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.JointPositionTarget", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.JointPositionTarget", JointPositionTarget) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONTARGET_H +#endif // GZ_SIM_COMPONENTS_JOINTPOSITIONTARGET_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/JointVelocityTarget.h b/scenario/src/gazebo/include/scenario/gazebo/components/JointVelocityTarget.h index fa21cd3b8..b8d90f07e 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/JointVelocityTarget.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/JointVelocityTarget.h @@ -24,19 +24,19 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYTARGET_H -#define IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYTARGET_H +#ifndef GZ_SIM_COMPONENTS_JOINTVELOCITYTARGET_H +#define GZ_SIM_COMPONENTS_JOINTVELOCITYTARGET_H #include -#include -#include -#include -#include +#include +#include +#include +#include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief Joint velocity target in SI units (rad/s for /// revolute, m/s for prismatic) used by joint @@ -48,11 +48,11 @@ namespace ignition::gazebo { Component, class JointVelocityTargetTag, serializers::VectorDoubleSerializer>; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.JointVelocityTarget", + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.JointVelocityTarget", JointVelocityTarget) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYTARGET_H +#endif // GZ_SIM_COMPONENTS_JOINTVELOCITYTARGET_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/SimulatedTime.h b/scenario/src/gazebo/include/scenario/gazebo/components/SimulatedTime.h index cf5315181..773b23a3c 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/SimulatedTime.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/SimulatedTime.h @@ -24,27 +24,27 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_SIMULATEDTIME_H -#define IGNITION_GAZEBO_COMPONENTS_SIMULATEDTIME_H +#ifndef GZ_SIM_COMPONENTS_SIMULATEDTIME_H +#define GZ_SIM_COMPONENTS_SIMULATEDTIME_H -#include -#include -#include +#include +#include +#include #include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief A component that holds the world's simulated time in /// seconds. using SimulatedTime = Component; - IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SimulatedTime", + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SimulatedTime", SimulatedTime) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_SIMULATEDTIME_H +#endif // GZ_SIM_COMPONENTS_SIMULATEDTIME_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/components/Timestamp.h b/scenario/src/gazebo/include/scenario/gazebo/components/Timestamp.h index 824458aaa..501203e93 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/components/Timestamp.h +++ b/scenario/src/gazebo/include/scenario/gazebo/components/Timestamp.h @@ -24,26 +24,26 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_TIMESTAMP_H -#define IGNITION_GAZEBO_COMPONENTS_TIMESTAMP_H +#ifndef GZ_SIM_COMPONENTS_TIMESTAMP_H +#define GZ_SIM_COMPONENTS_TIMESTAMP_H -#include -#include -#include +#include +#include +#include #include -namespace ignition::gazebo { +namespace gz::sim { // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { /// \brief A component that could store a timestamp. using Timestamp = Component; - IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Timestamp", + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.Timestamp", Timestamp) } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo + } // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace gz::sim -#endif // IGNITION_GAZEBO_COMPONENTS_TIMESTAMP_H +#endif // GZ_SIM_COMPONENTS_TIMESTAMP_H diff --git a/scenario/src/gazebo/include/scenario/gazebo/exceptions.h b/scenario/src/gazebo/include/scenario/gazebo/exceptions.h index ca1576dd2..6d04d50d1 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/exceptions.h +++ b/scenario/src/gazebo/include/scenario/gazebo/exceptions.h @@ -27,9 +27,9 @@ #ifndef SCENARIO_GAZEBO_EXCEPTIONS_H #define SCENARIO_GAZEBO_EXCEPTIONS_H -#include -#include -#include +#include +#include +#include #include #include @@ -254,13 +254,13 @@ class scenario::gazebo::exceptions::NotImplementedError class scenario::gazebo::exceptions::ComponentNotFound : public std::runtime_error { - ignition::gazebo::Entity entity; - ignition::gazebo::ComponentTypeId id; + gz::sim::Entity entity; + gz::sim::ComponentTypeId id; public: explicit ComponentNotFound( - const ignition::gazebo::ComponentTypeId id, - const ignition::gazebo::Entity entity = ignition::gazebo::kNullEntity) + const gz::sim::ComponentTypeId id, + const gz::sim::Entity entity = gz::sim::kNullEntity) : std::runtime_error("") , entity(entity) , id(id) @@ -270,13 +270,13 @@ class scenario::gazebo::exceptions::ComponentNotFound { std::string prefix; - if (entity != ignition::gazebo::kNullEntity) { + if (entity != gz::sim::kNullEntity) { prefix = "[Entity=" + std::to_string(entity) + "] "; } // Get the name of the component from the factory std::string componentName = - ignition::gazebo::components::Factory::Instance()->Name(id); + gz::sim::components::Factory::Instance()->Name(id); // Build the message std::string what = prefix + "Component not found: " + componentName; diff --git a/scenario/src/gazebo/include/scenario/gazebo/helpers.h b/scenario/src/gazebo/include/scenario/gazebo/helpers.h index 267a38c8f..f22d80c19 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/helpers.h +++ b/scenario/src/gazebo/include/scenario/gazebo/helpers.h @@ -33,17 +33,17 @@ #include "scenario/gazebo/World.h" #include "scenario/gazebo/exceptions.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -115,36 +115,36 @@ namespace scenario::gazebo::utils { }; template - auto getComponent(ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity, + auto getComponent(gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity, ComponentDataTypeT defaultValue = {}); template - auto getExistingComponent(ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity); + auto getExistingComponent(gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity); template - auto getComponentData(ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity) + auto getComponentData(gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity) -> decltype(ComponentTypeT().Data()); template - auto getExistingComponentData(ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity) + auto getExistingComponentData(gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity) -> decltype(ComponentTypeT().Data()); scenario::core::Pose - fromIgnitionPose(const ignition::math::Pose3d& ignitionPose); + fromGzPose(const gz::math::Pose3d& gzPose); - ignition::math::Pose3d toIgnitionPose(const scenario::core::Pose& pose); + gz::math::Pose3d toGzPose(const scenario::core::Pose& pose); scenario::core::Contact - fromIgnitionContactMsgs(ignition::gazebo::EntityComponentManager* ecm, - const ignition::msgs::Contact& contactMsg); + fromGzContactMsgs(gz::sim::EntityComponentManager* ecm, + const gz::msgs::Contact& contactMsg); std::vector - fromIgnitionContactsMsgs(ignition::gazebo::EntityComponentManager* ecm, - const ignition::msgs::Contacts& contactsMsg); + fromGzContactsMsgs(gz::sim::EntityComponentManager* ecm, + const gz::msgs::Contacts& contactsMsg); sdf::World renameSDFWorld(const sdf::World& world, const std::string& newWorldName); @@ -163,28 +163,28 @@ namespace scenario::gazebo::utils { sdf::JointType toSdf(const scenario::core::JointType type); scenario::core::JointType fromSdf(const sdf::JointType sdfType); - ignition::math::Vector3d fromModelToBaseLinearVelocity( - const ignition::math::Vector3d& linModelVelocity, - const ignition::math::Vector3d& angModelVelocity, - const ignition::math::Pose3d& M_H_B, - const ignition::math::Quaterniond& W_R_B); + gz::math::Vector3d fromModelToBaseLinearVelocity( + const gz::math::Vector3d& linModelVelocity, + const gz::math::Vector3d& angModelVelocity, + const gz::math::Pose3d& M_H_B, + const gz::math::Quaterniond& W_R_B); - ignition::math::Vector3d fromBaseToModelLinearVelocity( - const ignition::math::Vector3d& linBaseVelocity, - const ignition::math::Vector3d& angBaseVelocity, - const ignition::math::Pose3d& M_H_B, - const ignition::math::Quaterniond& W_R_B); + gz::math::Vector3d fromBaseToModelLinearVelocity( + const gz::math::Vector3d& linBaseVelocity, + const gz::math::Vector3d& angBaseVelocity, + const gz::math::Pose3d& M_H_B, + const gz::math::Quaterniond& W_R_B); std::shared_ptr getParentWorld(const GazeboEntity& gazeboEntity); std::shared_ptr getParentModel(const GazeboEntity& gazeboEntity); template - ignition::gazebo::Entity getFirstParentEntityWithComponent( - ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity) + gz::sim::Entity getFirstParentEntityWithComponent( + gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity) { - ignition::gazebo::Entity candidateEntity = entity; + gz::sim::Entity candidateEntity = entity; auto hasComponent = [&]() -> bool { return ecm->EntityHasComponentType(candidateEntity, @@ -192,7 +192,7 @@ namespace scenario::gazebo::utils { }; auto isNull = [&]() -> bool { - return candidateEntity == ignition::gazebo::kNullEntity; + return candidateEntity == gz::sim::kNullEntity; }; while (!(hasComponent() || isNull())) { @@ -210,8 +210,8 @@ namespace scenario::gazebo::utils { template auto setComponentData( - ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity, + gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity, const ComponentDataTypeT& data, const std::function& eql = @@ -219,48 +219,48 @@ namespace scenario::gazebo::utils { template auto setExistingComponentData( - ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity, + gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity, const ComponentDataTypeT& data, const std::function& eql = defaultEqualityOperator); static inline std::array - fromIgnitionVector(const ignition::math::Vector3d& ignitionVector) + fromGzVector(const gz::math::Vector3d& gzVector) { - return {ignitionVector.X(), ignitionVector.Y(), ignitionVector.Z()}; + return {gzVector.X(), gzVector.Y(), gzVector.Z()}; } - static inline std::array fromIgnitionQuaternion( - const ignition::math::Quaterniond& ignitionQuaternion) + static inline std::array fromGzQuaternion( + const gz::math::Quaterniond& gzQuaternion) { - return {ignitionQuaternion.W(), - ignitionQuaternion.X(), - ignitionQuaternion.Y(), - ignitionQuaternion.Z()}; + return {gzQuaternion.W(), + gzQuaternion.X(), + gzQuaternion.Y(), + gzQuaternion.Z()}; } - static inline ignition::math::Vector3d - toIgnitionVector3(const std::array& vector) + static inline gz::math::Vector3d + toGzVector3(const std::array& vector) { return {vector[0], vector[1], vector[2]}; } - static inline ignition::math::Vector4d - toIgnitionVector4(const std::array& vector) + static inline gz::math::Vector4d + toGzVector4(const std::array& vector) { return {vector[0], vector[1], vector[2], vector[3]}; } - static inline ignition::math::Quaterniond - toIgnitionQuaternion(const std::array& vector) + static inline gz::math::Quaterniond + toGzQuaternion(const std::array& vector) { return {vector[0], vector[1], vector[2], vector[3]}; } - static inline ignition::math::PID - toIgnitionPID(const scenario::core::PID& pid) + static inline gz::math::PID + toGzPID(const scenario::core::PID& pid) { return {pid.p, pid.i, @@ -273,7 +273,7 @@ namespace scenario::gazebo::utils { } static inline scenario::core::PID - fromIgnitionPID(const ignition::math::PID& pid) + fromGzPID(const gz::math::PID& pid) { scenario::core::PID pidScenario(pid.PGain(), pid.IGain(), pid.DGain()); pidScenario.cmdMin = pid.CmdMin(); @@ -289,7 +289,7 @@ namespace scenario::gazebo::utils { { public: WrenchWithDuration( - const ignition::msgs::Wrench& wrench, + const gz::msgs::Wrench& wrench, const std::chrono::steady_clock::duration& duration, const std::chrono::steady_clock::duration& curSimTime) : m_wrench(wrench) @@ -297,14 +297,14 @@ namespace scenario::gazebo::utils { {} WrenchWithDuration( - const ignition::math::Vector3d& force, - const ignition::math::Vector3d& torque, + const gz::math::Vector3d& force, + const gz::math::Vector3d& torque, const std::chrono::steady_clock::duration& duration, const std::chrono::steady_clock::duration& curSimTime) : m_expiration(curSimTime + duration) { - ignition::msgs::Set(m_wrench.mutable_force(), force); - ignition::msgs::Set(m_wrench.mutable_torque(), torque); + gz::msgs::Set(m_wrench.mutable_force(), force); + gz::msgs::Set(m_wrench.mutable_torque(), torque); } WrenchWithDuration( @@ -312,18 +312,18 @@ namespace scenario::gazebo::utils { const std::array& torque, const std::chrono::steady_clock::duration& duration, const std::chrono::steady_clock::duration& curSimTime) - : WrenchWithDuration(toIgnitionVector3(force), - toIgnitionVector3(torque), + : WrenchWithDuration(toGzVector3(force), + toGzVector3(torque), duration, curSimTime) {} - const ignition::msgs::Vector3d& force() const + const gz::msgs::Vector3d& force() const { return m_wrench.force(); } - const ignition::msgs::Vector3d& torque() const + const gz::msgs::Vector3d& torque() const { return m_wrench.torque(); } @@ -340,7 +340,7 @@ namespace scenario::gazebo::utils { } private: - ignition::msgs::Wrench m_wrench; + gz::msgs::Wrench m_wrench; std::chrono::steady_clock::duration m_expiration; }; @@ -356,9 +356,9 @@ namespace scenario::gazebo::utils { m_wrenches.push_back(wrench); } - inline ignition::msgs::Wrench totalWrench() const + inline gz::msgs::Wrench totalWrench() const { - using namespace ignition; + using namespace gz; msgs::Wrench totalWrench; msgs::Set(totalWrench.mutable_force(), {0, 0, 0}); @@ -399,8 +399,8 @@ namespace scenario::gazebo::utils { template auto scenario::gazebo::utils::getComponent( - ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity, + gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity, ComponentDataTypeT defaultValue) { if (!ecm) { @@ -419,8 +419,8 @@ auto scenario::gazebo::utils::getComponent( template auto scenario::gazebo::utils::getExistingComponent( - ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity) + gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity) { if (!ecm) { throw std::runtime_error("ECM pointer not valid"); @@ -437,8 +437,8 @@ auto scenario::gazebo::utils::getExistingComponent( template auto scenario::gazebo::utils::getComponentData( - ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity) -> decltype(ComponentTypeT().Data()) + gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity) -> decltype(ComponentTypeT().Data()) { using ComponentDataType = typename std::remove_reference::type; @@ -451,8 +451,8 @@ auto scenario::gazebo::utils::getComponentData( template auto scenario::gazebo::utils::getExistingComponentData( - ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity) -> decltype(ComponentTypeT().Data()) + gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity) -> decltype(ComponentTypeT().Data()) { auto component = getExistingComponent(ecm, entity); @@ -461,8 +461,8 @@ auto scenario::gazebo::utils::getExistingComponentData( template auto scenario::gazebo::utils::setComponentData( - ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity, + gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity, const ComponentDataTypeT& data, const std::function& eql) @@ -475,8 +475,8 @@ auto scenario::gazebo::utils::setComponentData( template auto scenario::gazebo::utils::setExistingComponentData( - ignition::gazebo::EntityComponentManager* ecm, - const ignition::gazebo::Entity entity, + gz::sim::EntityComponentManager* ecm, + const gz::sim::Entity entity, const ComponentDataTypeT& data, const std::function& eql) diff --git a/scenario/src/gazebo/include/scenario/gazebo/utils.h b/scenario/src/gazebo/include/scenario/gazebo/utils.h index 5d8cb0833..0c29c7c7a 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/utils.h +++ b/scenario/src/gazebo/include/scenario/gazebo/utils.h @@ -78,7 +78,7 @@ namespace scenario::gazebo::utils { /** * Find a SDF file in the filesystem. * - * The search path is defined with the ``IGN_GAZEBO_RESOURCE_PATH`` + * The search path is defined with the ``GZ_SIM_RESOURCE_PATH`` * environment variable. * * @param fileName The SDF file name. @@ -104,7 +104,7 @@ namespace scenario::gazebo::utils { * * @param fileName An SDF file. It could be either an absolute path * to the file or the file name if the parent folder is part - * of the ``IGN_GAZEBO_RESOURCE_PATH`` environment variable. + * of the ``GZ_SIM_RESOURCE_PATH`` environment variable. * @return The SDF string if the file was found and is valid, an * empty string otherwise. */ @@ -117,7 +117,7 @@ namespace scenario::gazebo::utils { * * @param fileName An SDF file. It could be either an absolute path * to the file or the file name if the parent folder is part - * of the ``IGN_GAZEBO_RESOURCE_PATH`` environment variable. + * of the ``GZ_SIM_RESOURCE_PATH`` environment variable. * @return The name of the model if the file was found and is valid, * an empty string otherwise. */ @@ -128,7 +128,7 @@ namespace scenario::gazebo::utils { * * @param fileName An SDF file. It could be either an absolute path * to the file or the file name if the parent folder is part - * of the ``IGN_GAZEBO_RESOURCE_PATH`` environment variable. + * of the ``GZ_SIM_RESOURCE_PATH`` environment variable. * @param worldIndex The index of the world in the SDF file. By * default it finds the first world. * @return The name of the world if the file was found and is valid, diff --git a/scenario/src/gazebo/src/GazeboSimulator.cpp b/scenario/src/gazebo/src/GazeboSimulator.cpp index 790c2e149..257b2106a 100644 --- a/scenario/src/gazebo/src/GazeboSimulator.cpp +++ b/scenario/src/gazebo/src/GazeboSimulator.cpp @@ -34,17 +34,17 @@ #include "scenario/gazebo/helpers.h" #include "scenario/gazebo/utils.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -65,8 +65,8 @@ namespace scenario::gazebo::detail { struct PhysicsData; struct SimulationResources { - ignition::gazebo::EventManager* eventMgr = nullptr; - ignition::gazebo::EntityComponentManager* ecm = nullptr; + gz::sim::EventManager* eventMgr = nullptr; + gz::sim::EntityComponentManager* ecm = nullptr; }; } // namespace scenario::gazebo::detail @@ -80,22 +80,22 @@ struct detail::PhysicsData }; class scenario::gazebo::detail::ECMProvider final - : public ignition::gazebo::System - , public ignition::gazebo::ISystemConfigure + : public gz::sim::System + , public gz::sim::ISystemConfigure { public: ECMProvider() - : ignition::gazebo::System() + : gz::sim::System() {} - void Configure(const ignition::gazebo::Entity& entity, + void Configure(const gz::sim::Entity& entity, const std::shared_ptr& /*sdf*/, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr); + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& eventMgr); std::string worldName; - ignition::gazebo::EventManager* eventMgr = nullptr; - ignition::gazebo::EntityComponentManager* ecm = nullptr; + gz::sim::EventManager* eventMgr = nullptr; + gz::sim::EntityComponentManager* ecm = nullptr; }; // ============== @@ -112,7 +112,7 @@ class GazeboSimulator::Impl detail::PhysicsData physics; uint64_t numOfIterations = 0; std::unique_ptr gui; - std::shared_ptr server; + std::shared_ptr server; } gazebo; using WorldName = std::string; @@ -122,7 +122,7 @@ class GazeboSimulator::Impl std::unordered_map resources; bool insertSDFWorld(const sdf::World& world); - std::shared_ptr getServer(); + std::shared_ptr getServer(); static std::shared_ptr CreateGazeboWorld(const std::string& worldName, @@ -149,9 +149,9 @@ GazeboSimulator::GazeboSimulator(const double stepSize, // Configure Fuel Callback sdf::setFindCallback([](const std::string& uri) -> std::string { - auto fuelClient = ignition::fuel_tools::FuelClient(); + auto fuelClient = gz::fuel_tools::FuelClient(); const auto path = - ignition::fuel_tools::fetchResourceWithClient(uri, fuelClient); + gz::fuel_tools::fetchResourceWithClient(uri, fuelClient); return path; }); } @@ -172,7 +172,7 @@ double GazeboSimulator::stepSize() const // Get the active physics parameters const auto& physics = utils::getExistingComponentData< // - ignition::gazebo::components::Physics>(world->ecm(), world->entity()); + gz::sim::components::Physics>(world->ecm(), world->entity()); return physics.MaxStepSize(); } @@ -188,7 +188,7 @@ double GazeboSimulator::realTimeFactor() const // Get the active physics parameters const auto& physics = utils::getExistingComponentData< // - ignition::gazebo::components::Physics>(world->ecm(), world->entity()); + gz::sim::components::Physics>(world->ecm(), world->entity()); return physics.RealTimeFactor(); } @@ -241,7 +241,7 @@ bool GazeboSimulator::run(const bool paused) // Get the gazebo server auto server = pImpl->getServer(); if (!server) { - sError << "Failed to get the ignition server" << std::endl; + sError << "Failed to get the gz server" << std::endl; return false; } @@ -265,7 +265,7 @@ bool GazeboSimulator::run(const bool paused) iterations = 1; } - // Recent versions of Ignition Gazebo optimize the streaming of pose updates + // Recent versions of Gz Gazebo optimize the streaming of pose updates // in order to reduce the bandwidth between server and client. // However, it does not take into account paused steps. // Here below we force all the Pose components to be streamed by manually @@ -281,13 +281,13 @@ bool GazeboSimulator::run(const bool paused) auto* ecm = this->pImpl->resources.at(worldName).ecm; // Mark all all entities with Pose component as Changed - ecm->Each( - [&](const ignition::gazebo::Entity& entity, - ignition::gazebo::components::Pose*) -> bool { + ecm->Each( + [&](const gz::sim::Entity& entity, + gz::sim::components::Pose*) -> bool { ecm->SetChanged( entity, - ignition::gazebo::components::Pose::typeId, - ignition::gazebo::ComponentState::OneTimeChange); + gz::sim::components::Pose::typeId, + gz::sim::ComponentState::OneTimeChange); return true; }); } @@ -318,7 +318,7 @@ bool GazeboSimulator::gui(const int verbosity) return false; } - // If ign-gazebo-gui is already running, return without doing anything + // If gz-sim-gui is already running, return without doing anything int exit_status; if (pImpl->gazebo.gui && !pImpl->gazebo.gui->try_get_exit_status(exit_status)) { @@ -339,8 +339,8 @@ bool GazeboSimulator::gui(const int verbosity) sDebug << "Starting the SceneBroadcaster plugin" << std::endl; auto world = this->getWorld(worldName); if (!std::static_pointer_cast(world)->insertWorldPlugin( - "ignition-gazebo-scene-broadcaster-system", - "ignition::gazebo::systems::SceneBroadcaster")) { + "gz-sim-scene-broadcaster-system", + "gz::sim::systems::SceneBroadcaster")) { sError << "Failed to load SceneBroadcaster plugin" << std::endl; return false; } @@ -351,7 +351,7 @@ bool GazeboSimulator::gui(const int verbosity) if (guiVerbosity < 0) { // Get the verbosity level - guiVerbosity = ignition::common::Console::Verbosity(); + guiVerbosity = gz::common::Console::Verbosity(); } #if defined(WIN32) || defined(_WIN32) @@ -364,10 +364,10 @@ bool GazeboSimulator::gui(const int verbosity) // Spawn a new process with the GUI pImpl->gazebo.gui = std::make_unique( - "ign gazebo -g -v " + std::to_string(guiVerbosity) + redirect); + "gz sim -g -v " + std::to_string(guiVerbosity) + redirect); bool guiServiceExists = false; - ignition::transport::Node node; + gz::transport::Node node; std::vector serviceList; do { @@ -602,20 +602,20 @@ GazeboSimulator::getWorld(const std::string& worldName) const // ============== void detail::ECMProvider::Configure( - const ignition::gazebo::Entity& entity, + const gz::sim::Entity& entity, const std::shared_ptr&, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& eventMgr) { if (!ecm.EntityHasComponentType( - entity, ignition::gazebo::components::World::typeId)) { + entity, gz::sim::components::World::typeId)) { sError << "The ECMProvider system was not inserted " << "in a world element" << std::endl; return; } this->worldName = utils::getExistingComponentData< // - ignition::gazebo::components::Name>(&ecm, entity); + gz::sim::components::Name>(&ecm, entity); this->ecm = &ecm; this->eventMgr = &eventMgr; @@ -650,7 +650,7 @@ bool GazeboSimulator::Impl::insertSDFWorld(const sdf::World& world) return true; } -std::shared_ptr GazeboSimulator::Impl::getServer() +std::shared_ptr GazeboSimulator::Impl::getServer() { // Lazy initialization of the server if (gazebo.server) { @@ -698,16 +698,16 @@ std::shared_ptr GazeboSimulator::Impl::getServer() // https://github.com/ignitionrobotics/ign-gazebo/pull/281 // TODO: this will not likely work in Windows. std::string value; - if (!ignition::common::env( - ignition::gazebo::kServerConfigPathEnv, value, true) - && !ignition::common::setenv(ignition::gazebo::kServerConfigPathEnv, + if (!gz::common::env( + gz::sim::kServerConfigPathEnv, value, true) + && !gz::common::setenv(gz::sim::kServerConfigPathEnv, "")) { - sError << "Failed to set " << ignition::gazebo::kServerConfigPathEnv + sError << "Failed to set " << gz::sim::kServerConfigPathEnv << std::endl; return nullptr; } - ignition::gazebo::ServerConfig config; + gz::sim::ServerConfig config; config.SetSeed(0); config.SetUseLevels(false); config.SetSdfString(root.Element()->ToString("")); @@ -716,7 +716,7 @@ std::shared_ptr GazeboSimulator::Impl::getServer() // The worlds are initialized with the physics parameters // (rtf and physics step) defined in the SDF. // They get overridden below. - auto server = std::make_shared(config); + auto server = std::make_shared(config); assert(server); // Add a Configure-only system to get the ECM pointer @@ -758,12 +758,12 @@ std::shared_ptr GazeboSimulator::Impl::getServer() // Get the world entity const auto worldEntity = resources.ecm->EntityByComponents( - ignition::gazebo::components::World(), - ignition::gazebo::components::Name(worldName)); + gz::sim::components::World(), + gz::sim::components::Name(worldName)); // Create a new PhysicsCmd component auto& physics = - utils::getComponentData( + utils::getComponentData( resources.ecm, worldEntity); // Store the physics parameters. @@ -819,8 +819,8 @@ std::shared_ptr GazeboSimulator::Impl::CreateGazeboWorld( { // Get the world entity const auto worldEntity = resources.ecm->EntityByComponents( - ignition::gazebo::components::World(), - ignition::gazebo::components::Name(worldName)); + gz::sim::components::World(), + gz::sim::components::Name(worldName)); // Create the world object auto world = std::make_shared(); @@ -845,8 +845,8 @@ std::shared_ptr GazeboSimulator::Impl::CreateGazeboWorld( bool GazeboSimulator::Impl::sceneBroadcasterActive(const std::string& worldName) { - ignition::transport::Node node; - std::vector publishers; + gz::transport::Node node; + std::vector publishers; std::string serviceName{"/world/" + worldName + "/scene/info"}; node.ServiceInfo(serviceName, publishers); diff --git a/scenario/src/gazebo/src/Joint.cpp b/scenario/src/gazebo/src/Joint.cpp index 531640d63..a980e7980 100644 --- a/scenario/src/gazebo/src/Joint.cpp +++ b/scenario/src/gazebo/src/Joint.cpp @@ -42,18 +42,18 @@ #include "scenario/gazebo/helpers.h" #include "scenario/gazebo/utils.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -61,7 +61,7 @@ #include using namespace scenario::gazebo; -const ignition::math::PID DefaultPID(1, 0.1, 0.01, -1, 0, -1, 0, 0); +const gz::math::PID DefaultPID(1, 0.1, 0.01, -1, 0, -1, 0, 0); class Joint::Impl { @@ -88,11 +88,11 @@ uint64_t Joint::id() const return std::hash{}(scopedJointName); } -bool Joint::initialize(const ignition::gazebo::Entity jointEntity, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventManager) +bool Joint::initialize(const gz::sim::Entity jointEntity, + gz::sim::EntityComponentManager* ecm, + gz::sim::EventManager* eventManager) { - if (jointEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { + if (jointEntity == gz::sim::kNullEntity || !ecm || !eventManager) { sError << "Failed to initialize Joint" << std::endl; return false; } @@ -114,7 +114,7 @@ bool Joint::createECMResources() { sMessage << " [" << m_entity << "] " << this->name() << std::endl; - using namespace ignition::gazebo; + using namespace gz::sim; const std::vector zero(this->dofs(), 0.0); @@ -147,7 +147,7 @@ bool Joint::resetPosition(const double position, size_t dof) } auto& jointPositionReset = utils::getComponentData< // - ignition::gazebo::components::JointPositionReset>(m_ecm, m_entity); + gz::sim::components::JointPositionReset>(m_ecm, m_entity); if (jointPositionReset.size() != this->dofs()) { assert(jointPositionReset.size() == 0); @@ -156,7 +156,7 @@ bool Joint::resetPosition(const double position, size_t dof) // Reset the PID auto& pid = utils::getExistingComponentData< // - ignition::gazebo::components::JointPID>(m_ecm, m_entity); + gz::sim::components::JointPID>(m_ecm, m_entity); pid.Reset(); jointPositionReset[dof] = position; @@ -172,7 +172,7 @@ bool Joint::resetVelocity(const double velocity, const size_t dof) } auto& jointVelocityReset = utils::getComponentData< // - ignition::gazebo::components::JointVelocityReset>(m_ecm, m_entity); + gz::sim::components::JointVelocityReset>(m_ecm, m_entity); if (jointVelocityReset.size() != this->dofs()) { assert(jointVelocityReset.size() == 0); @@ -181,7 +181,7 @@ bool Joint::resetVelocity(const double velocity, const size_t dof) // Reset the PID auto& pid = utils::getExistingComponentData< // - ignition::gazebo::components::JointPID>(m_ecm, m_entity); + gz::sim::components::JointPID>(m_ecm, m_entity); pid.Reset(); jointVelocityReset[dof] = velocity; @@ -214,14 +214,14 @@ bool Joint::resetJointPosition(const std::vector& position) } auto& jointPositionReset = utils::getComponentData< // - ignition::gazebo::components::JointPositionReset>(m_ecm, m_entity); + gz::sim::components::JointPositionReset>(m_ecm, m_entity); // Update the position jointPositionReset = position; // Reset the PID auto& pid = utils::getExistingComponentData< // - ignition::gazebo::components::JointPID>(m_ecm, m_entity); + gz::sim::components::JointPID>(m_ecm, m_entity); pid.Reset(); return true; @@ -236,14 +236,14 @@ bool Joint::resetJointVelocity(const std::vector& velocity) } auto& jointVelocityReset = utils::getComponentData< // - ignition::gazebo::components::JointVelocityReset>(m_ecm, m_entity); + gz::sim::components::JointVelocityReset>(m_ecm, m_entity); // Update the velocity jointVelocityReset = velocity; // Reset the PID auto& pid = utils::getExistingComponentData< // - ignition::gazebo::components::JointPID>(m_ecm, m_entity); + gz::sim::components::JointPID>(m_ecm, m_entity); pid.Reset(); return true; @@ -278,7 +278,7 @@ bool Joint::setCoulombFriction(const double value) case core::JointType::Prismatic: case core::JointType::Ball: { sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); axis.SetFriction(value); return true; } @@ -305,7 +305,7 @@ bool Joint::setViscousFriction(const double value) case core::JointType::Prismatic: case core::JointType::Ball: { sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); axis.SetDamping(value); return true; } @@ -346,7 +346,7 @@ size_t Joint::dofs() const std::string Joint::name(const bool scoped) const { std::string jointName = utils::getExistingComponentData< // - ignition::gazebo::components::Name>(m_ecm, m_entity); + gz::sim::components::Name>(m_ecm, m_entity); if (scoped) { jointName = utils::getParentModel(*this)->name() + "::" + jointName; @@ -359,7 +359,7 @@ scenario::core::JointType Joint::type() const { // Get the joint type const sdf::JointType sdfType = utils::getExistingComponentData< // - ignition::gazebo::components::JointType>(m_ecm, m_entity); + gz::sim::components::JointType>(m_ecm, m_entity); // Convert the joint type const core::JointType type = utils::fromSdf(sdfType); @@ -370,7 +370,7 @@ scenario::core::JointType Joint::type() const scenario::core::JointControlMode Joint::controlMode() const { const core::JointControlMode mode = utils::getExistingComponentData< - ignition::gazebo::components::JointControlMode>(m_ecm, m_entity); + gz::sim::components::JointControlMode>(m_ecm, m_entity); return mode; } @@ -401,7 +401,7 @@ bool Joint::setControlMode(const scenario::core::JointControlMode mode) // Insert the plugin if the model does not have it already if (!m_ecm->EntityHasComponentType( parentModel->entity(), - ignition::gazebo::components::JointController::typeId)) { + gz::sim::components::JointController::typeId)) { sDebug << "Loading JointController plugin for model '" << parentModel->name() << "'" << std::endl; @@ -418,41 +418,41 @@ bool Joint::setControlMode(const scenario::core::JointControlMode mode) } utils::setExistingComponentData< - ignition::gazebo::components::JointControlMode>(m_ecm, m_entity, mode); + gz::sim::components::JointControlMode>(m_ecm, m_entity, mode); // Delete the existing targets if they exist sDebug << "Deleting existing targets after changing control mode" << std::endl; m_ecm->RemoveComponent( - m_entity, ignition::gazebo::components::JointPositionTarget::typeId); + m_entity, gz::sim::components::JointPositionTarget::typeId); m_ecm->RemoveComponent( - m_entity, ignition::gazebo::components::JointVelocityTarget::typeId); + m_entity, gz::sim::components::JointVelocityTarget::typeId); m_ecm->RemoveComponent( m_entity, - ignition::gazebo::components::JointAccelerationTarget::typeId); + gz::sim::components::JointAccelerationTarget::typeId); m_ecm->RemoveComponent( - m_entity, ignition::gazebo::components::JointVelocityCmd::typeId); + m_entity, gz::sim::components::JointVelocityCmd::typeId); m_ecm->RemoveComponent(m_entity, - ignition::gazebo::components::JointForceCmd::typeId); + gz::sim::components::JointForceCmd::typeId); // Initialize the target as the current position / velocity switch (mode) { case core::JointControlMode::Position: case core::JointControlMode::PositionInterpolated: utils::setComponentData< - ignition::gazebo::components::JointPositionTarget>( + gz::sim::components::JointPositionTarget>( m_ecm, m_entity, this->jointPosition()); break; case core::JointControlMode::Velocity: case core::JointControlMode::VelocityFollowerDart: utils::setComponentData< - ignition::gazebo::components::JointVelocityTarget>( + gz::sim::components::JointVelocityTarget>( m_ecm, m_entity, this->jointVelocity()); break; case core::JointControlMode::Idle: case core::JointControlMode::Force: utils::setComponentData< - ignition::gazebo::components::JointForceCmd>( + gz::sim::components::JointForceCmd>( m_ecm, m_entity, std::vector(this->dofs(), 0.0)); break; case core::JointControlMode::Invalid: @@ -462,7 +462,7 @@ bool Joint::setControlMode(const scenario::core::JointControlMode mode) // Reset the PID auto& pid = utils::getExistingComponentData< // - ignition::gazebo::components::JointPID>(m_ecm, m_entity); + gz::sim::components::JointPID>(m_ecm, m_entity); pid.Reset(); return true; @@ -471,7 +471,7 @@ bool Joint::setControlMode(const scenario::core::JointControlMode mode) double Joint::controllerPeriod() const { auto duration = utils::getExistingComponentData< // - ignition::gazebo::components::JointControllerPeriod>( + gz::sim::components::JointControllerPeriod>( m_ecm, m_ecm->ParentEntity(m_entity)); return utils::steadyClockDurationToDouble(duration); @@ -479,16 +479,16 @@ double Joint::controllerPeriod() const scenario::core::PID Joint::pid() const { - const ignition::math::PID& pid = utils::getExistingComponentData< // - ignition::gazebo::components::JointPID>(m_ecm, m_entity); + const gz::math::PID& pid = utils::getExistingComponentData< // + gz::sim::components::JointPID>(m_ecm, m_entity); - return utils::fromIgnitionPID(pid); + return utils::fromGzPID(pid); } bool Joint::setPID(const scenario::core::PID& pid) { - auto eqOp = [](const ignition::math::PID& a, - const ignition::math::PID& b) -> bool { + auto eqOp = [](const gz::math::PID& a, + const gz::math::PID& b) -> bool { bool equal = true; const double epsilon = std::numeric_limits::epsilon(); @@ -522,13 +522,13 @@ bool Joint::setPID(const scenario::core::PID& pid) } // Create the new PID - const ignition::math::PID& pidIgnitionMath = - utils::toIgnitionPID(pidParams); + const gz::math::PID& pidGzMath = + utils::toGzPID(pidParams); // Store the new PID in the ECM - utils::setExistingComponentData( - m_ecm, m_entity, pidIgnitionMath, eqOp); + utils::setExistingComponentData( + m_ecm, m_entity, pidGzMath, eqOp); return true; } @@ -537,7 +537,7 @@ bool Joint::historyOfAppliedJointForcesEnabled() const { return m_ecm->EntityHasComponentType( m_entity, - ignition::gazebo::components::HistoryOfAppliedJointForces::typeId); + gz::sim::components::HistoryOfAppliedJointForces::typeId); } bool Joint::enableHistoryOfAppliedJointForces(const bool enable, @@ -546,13 +546,13 @@ bool Joint::enableHistoryOfAppliedJointForces(const bool enable, if (enable) { // If the component already exists, its value is not overridden utils::getComponent< - ignition::gazebo::components::HistoryOfAppliedJointForces>( + gz::sim::components::HistoryOfAppliedJointForces>( m_ecm, m_entity, utils::FixedSizeQueue(maxHistorySize)); } else { m_ecm->RemoveComponent( m_entity, - ignition::gazebo::components::HistoryOfAppliedJointForces::typeId); + gz::sim::components::HistoryOfAppliedJointForces::typeId); } return true; @@ -565,7 +565,7 @@ std::vector Joint::historyOfAppliedJointForces() const } const auto& fixedSizeQueue = utils::getExistingComponentData< - ignition::gazebo::components::HistoryOfAppliedJointForces>(m_ecm, + gz::sim::components::HistoryOfAppliedJointForces>(m_ecm, m_entity); return fixedSizeQueue.toStdVector(); @@ -578,7 +578,7 @@ double Joint::coulombFriction() const case core::JointType::Prismatic: case core::JointType::Ball: { const sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); return axis.Friction(); } case core::JointType::Fixed: @@ -599,7 +599,7 @@ double Joint::viscousFriction() const case core::JointType::Prismatic: case core::JointType::Ball: { const sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); return axis.Damping(); } case core::JointType::Fixed: @@ -737,7 +737,7 @@ bool Joint::setPositionTarget(const double position, const size_t dof) } auto& jointPositionTarget = utils::getComponentData< // - ignition::gazebo::components::JointPositionTarget>(m_ecm, m_entity); + gz::sim::components::JointPositionTarget>(m_ecm, m_entity); if (jointPositionTarget.size() != this->dofs()) { assert(jointPositionTarget.size() == 0); @@ -765,7 +765,7 @@ bool Joint::setVelocityTarget(const double velocity, const size_t dof) } auto& jointVelocityTarget = utils::getComponentData< // - ignition::gazebo::components::JointVelocityTarget>(m_ecm, m_entity); + gz::sim::components::JointVelocityTarget>(m_ecm, m_entity); if (jointVelocityTarget.size() != this->dofs()) { assert(jointVelocityTarget.size() == 0); @@ -792,7 +792,7 @@ bool Joint::setAccelerationTarget(const double acceleration, const size_t dof) } auto& jointAccelerationTarget = utils::getComponentData< // - ignition::gazebo::components::JointAccelerationTarget>(m_ecm, m_entity); + gz::sim::components::JointAccelerationTarget>(m_ecm, m_entity); if (jointAccelerationTarget.size() != this->dofs()) { assert(jointAccelerationTarget.size() == 0); @@ -828,7 +828,7 @@ bool Joint::setGeneralizedForceTarget(const double force, const size_t dof) } auto& jointForce = utils::getComponentData< // - ignition::gazebo::components::JointForceCmd>(m_ecm, m_entity); + gz::sim::components::JointForceCmd>(m_ecm, m_entity); if (jointForce.size() != this->dofs()) { assert(jointForce.size() == 0); @@ -895,7 +895,7 @@ scenario::core::JointLimit Joint::jointPositionLimit() const case core::JointType::Revolute: case core::JointType::Prismatic: { sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); jointLimit.min[0] = axis.Lower(); jointLimit.max[0] = axis.Upper(); break; @@ -922,7 +922,7 @@ scenario::core::JointLimit Joint::jointVelocityLimit() const case core::JointType::Revolute: case core::JointType::Prismatic: { sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); jointLimit.min[0] = -axis.MaxVelocity(); jointLimit.max[0] = axis.MaxVelocity(); break; @@ -959,7 +959,7 @@ bool Joint::setJointVelocityLimit(const std::vector& maxVelocity) case core::JointType::Revolute: case core::JointType::Prismatic: { sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); axis.SetMaxVelocity(maxVelocity[0]); return true; } @@ -977,7 +977,7 @@ bool Joint::setJointVelocityLimit(const std::vector& maxVelocity) } sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); axis.SetMaxVelocity(maxVelocity0); return true; } @@ -999,7 +999,7 @@ std::vector Joint::jointMaxGeneralizedForce() const case core::JointType::Revolute: case core::JointType::Prismatic: { const sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); maxGeneralizedForce = {axis.Effort()}; break; } @@ -1033,7 +1033,7 @@ bool Joint::setJointMaxGeneralizedForce(const std::vector& maxForce) case core::JointType::Prismatic: case core::JointType::Ball: { sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(m_ecm, m_entity); + gz::sim::components::JointAxis>(m_ecm, m_entity); assert(maxForce.size() == 1); axis.SetEffort(maxForce[0]); return true; @@ -1051,7 +1051,7 @@ bool Joint::setJointMaxGeneralizedForce(const std::vector& maxForce) std::vector Joint::jointPosition() const { const std::vector& jointPosition = utils::getExistingComponentData< - ignition::gazebo::components::JointPosition>(m_ecm, m_entity); + gz::sim::components::JointPosition>(m_ecm, m_entity); if (jointPosition.size() != this->dofs()) { throw exceptions::DOFMismatch( @@ -1064,7 +1064,7 @@ std::vector Joint::jointPosition() const std::vector Joint::jointVelocity() const { const std::vector& jointVelocity = utils::getExistingComponentData< - ignition::gazebo::components::JointVelocity>(m_ecm, m_entity); + gz::sim::components::JointVelocity>(m_ecm, m_entity); if (jointVelocity.size() != this->dofs()) { throw exceptions::DOFMismatch( @@ -1078,7 +1078,7 @@ std::vector Joint::jointAcceleration() const { const std::vector& jointAcceleration = utils::getExistingComponentData< // - ignition::gazebo::components::JointAcceleration>(m_ecm, m_entity); + gz::sim::components::JointAcceleration>(m_ecm, m_entity); if (jointAcceleration.size() != this->dofs()) { throw exceptions::DOFMismatch( @@ -1092,7 +1092,7 @@ std::vector Joint::jointGeneralizedForce() const { const std::vector& jointGeneralizedForce = utils::getExistingComponentData< - ignition::gazebo::components::JointForce>(m_ecm, m_entity); + gz::sim::components::JointForce>(m_ecm, m_entity); if (jointGeneralizedForce.size() != this->dofs()) { throw exceptions::DOFMismatch( @@ -1111,7 +1111,7 @@ bool Joint::setJointPositionTarget(const std::vector& position) } auto& jointPositionTarget = utils::getComponentData< // - ignition::gazebo::components::JointPositionTarget>(m_ecm, m_entity); + gz::sim::components::JointPositionTarget>(m_ecm, m_entity); jointPositionTarget = position; return true; @@ -1126,7 +1126,7 @@ bool Joint::setJointVelocityTarget(const std::vector& velocity) } auto& jointVelocityTarget = utils::getComponentData< // - ignition::gazebo::components::JointVelocityTarget>(m_ecm, m_entity); + gz::sim::components::JointVelocityTarget>(m_ecm, m_entity); jointVelocityTarget = velocity; return true; @@ -1141,7 +1141,7 @@ bool Joint::setJointAccelerationTarget(const std::vector& acceleration) } auto& jointAccelerationTarget = utils::getComponentData< // - ignition::gazebo::components::JointAccelerationTarget>(m_ecm, m_entity); + gz::sim::components::JointAccelerationTarget>(m_ecm, m_entity); jointAccelerationTarget = acceleration; return true; @@ -1156,7 +1156,7 @@ bool Joint::setJointGeneralizedForceTarget(const std::vector& force) } auto& jointForceTarget = utils::getComponentData< // - ignition::gazebo::components::JointForceCmd>(m_ecm, m_entity); + gz::sim::components::JointForceCmd>(m_ecm, m_entity); const std::vector& maxForce = this->jointMaxGeneralizedForce(); @@ -1175,7 +1175,7 @@ bool Joint::setJointGeneralizedForceTarget(const std::vector& force) std::vector Joint::jointPositionTarget() const { const std::vector& target = utils::getExistingComponentData< - ignition::gazebo::components::JointPositionTarget>(m_ecm, m_entity); + gz::sim::components::JointPositionTarget>(m_ecm, m_entity); if (target.size() != this->dofs()) { throw exceptions::DOFMismatch( @@ -1188,7 +1188,7 @@ std::vector Joint::jointPositionTarget() const std::vector Joint::jointVelocityTarget() const { const std::vector& target = utils::getExistingComponentData< - ignition::gazebo::components::JointVelocityTarget>(m_ecm, m_entity); + gz::sim::components::JointVelocityTarget>(m_ecm, m_entity); if (target.size() != this->dofs()) { throw exceptions::DOFMismatch( @@ -1201,7 +1201,7 @@ std::vector Joint::jointVelocityTarget() const std::vector Joint::jointAccelerationTarget() const { const std::vector& target = utils::getExistingComponentData< - ignition::gazebo::components::JointAccelerationTarget>(m_ecm, m_entity); + gz::sim::components::JointAccelerationTarget>(m_ecm, m_entity); if (target.size() != this->dofs()) { throw exceptions::DOFMismatch( @@ -1214,7 +1214,7 @@ std::vector Joint::jointAccelerationTarget() const std::vector Joint::jointGeneralizedForceTarget() const { const std::vector& target = utils::getExistingComponentData< // - ignition::gazebo::components::JointForceCmd>(m_ecm, m_entity); + gz::sim::components::JointForceCmd>(m_ecm, m_entity); if (target.size() != this->dofs()) { throw exceptions::DOFMismatch( diff --git a/scenario/src/gazebo/src/Link.cpp b/scenario/src/gazebo/src/Link.cpp index 0d0cb9f63..f74a43711 100644 --- a/scenario/src/gazebo/src/Link.cpp +++ b/scenario/src/gazebo/src/Link.cpp @@ -34,23 +34,23 @@ #include "scenario/gazebo/helpers.h" #include "scenario/gazebo/utils.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -61,9 +61,9 @@ using namespace scenario::gazebo; class Link::Impl { public: - ignition::gazebo::Link link; + gz::sim::Link link; - static ignition::math::Pose3d GetWorldPose(const Link& link, + static gz::math::Pose3d GetWorldPose(const Link& link, const Link::Impl& impl) { if (!impl.link.IsCanonical(*link.ecm())) { @@ -81,17 +81,17 @@ class Link::Impl impl.link.ParentModel(*link.ecm()); assert(parentModelOptional.has_value()); - const ignition::gazebo::Model& parentModel = + const gz::sim::Model& parentModel = parentModelOptional.value(); - const ignition::gazebo::Entity parentModelEntity = + const gz::sim::Entity parentModelEntity = parentModel.Entity(); auto W_H_M = utils::getExistingComponentData< - ignition::gazebo::components::Pose>(link.ecm(), + gz::sim::components::Pose>(link.ecm(), parentModelEntity); auto M_H_B = utils::getExistingComponentData< - ignition::gazebo::components::Pose>(link.ecm(), link.entity()); + gz::sim::components::Pose>(link.ecm(), link.entity()); return W_H_M * M_H_B; } @@ -118,11 +118,11 @@ uint64_t Link::id() const return std::hash{}(scopedLinkName); } -bool Link::initialize(const ignition::gazebo::Entity linkEntity, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventManager) +bool Link::initialize(const gz::sim::Entity linkEntity, + gz::sim::EntityComponentManager* ecm, + gz::sim::EventManager* eventManager) { - if (linkEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { + if (linkEntity == gz::sim::kNullEntity || !ecm || !eventManager) { sError << "Failed to initialize Link" << std::endl; return false; } @@ -131,7 +131,7 @@ bool Link::initialize(const ignition::gazebo::Entity linkEntity, m_entity = linkEntity; m_eventManager = eventManager; - pImpl->link = ignition::gazebo::Link(linkEntity); + pImpl->link = gz::sim::Link(linkEntity); // Check that the link is valid if (!pImpl->link.Valid(*ecm)) { @@ -146,7 +146,7 @@ bool Link::createECMResources() { sMessage << " [" << m_entity << "] " << this->name() << std::endl; - using namespace ignition::gazebo; + using namespace gz::sim; // Create link components m_ecm->CreateComponent(m_entity, // @@ -201,21 +201,21 @@ std::string Link::name(const bool scoped) const double Link::mass() const { const auto& inertial = utils::getExistingComponentData< // - ignition::gazebo::components::Inertial>(m_ecm, m_entity); + gz::sim::components::Inertial>(m_ecm, m_entity); return inertial.MassMatrix().Mass(); } std::array Link::position() const { - const ignition::math::Pose3d& linkPose = Impl::GetWorldPose(*this, *pImpl); - return utils::fromIgnitionPose(linkPose).position; + const gz::math::Pose3d& linkPose = Impl::GetWorldPose(*this, *pImpl); + return utils::fromGzPose(linkPose).position; } std::array Link::orientation() const { - const ignition::math::Pose3d& linkPose = Impl::GetWorldPose(*this, *pImpl); - return utils::fromIgnitionPose(linkPose).orientation; + const gz::math::Pose3d& linkPose = Impl::GetWorldPose(*this, *pImpl); + return utils::fromGzPose(linkPose).orientation; } std::array Link::worldLinearVelocity() const @@ -227,7 +227,7 @@ std::array Link::worldLinearVelocity() const this->name()); } - return utils::fromIgnitionVector(linkLinearVelocity.value()); + return utils::fromGzVector(linkLinearVelocity.value()); } std::array Link::worldAngularVelocity() const @@ -239,23 +239,23 @@ std::array Link::worldAngularVelocity() const this->name()); } - return utils::fromIgnitionVector(linkAngularVelocity.value()); + return utils::fromGzVector(linkAngularVelocity.value()); } std::array Link::bodyLinearVelocity() const { const auto& linkBodyLinVel = utils::getComponentData< // - ignition::gazebo::components::LinearVelocity>(m_ecm, m_entity); + gz::sim::components::LinearVelocity>(m_ecm, m_entity); - return utils::fromIgnitionVector(linkBodyLinVel); + return utils::fromGzVector(linkBodyLinVel); } std::array Link::bodyAngularVelocity() const { const auto& linkBodyAngVel = utils::getComponentData< // - ignition::gazebo::components::AngularVelocity>(m_ecm, m_entity); + gz::sim::components::AngularVelocity>(m_ecm, m_entity); - return utils::fromIgnitionVector(linkBodyAngVel); + return utils::fromGzVector(linkBodyAngVel); } std::array Link::worldLinearAcceleration() const @@ -268,40 +268,40 @@ std::array Link::worldLinearAcceleration() const this->name()); } - return utils::fromIgnitionVector(linkLinearAcceleration.value()); + return utils::fromGzVector(linkLinearAcceleration.value()); } std::array Link::worldAngularAcceleration() const { const auto& linkWorldAngAcc = utils::getComponentData< - ignition::gazebo::components::WorldAngularAcceleration>(m_ecm, + gz::sim::components::WorldAngularAcceleration>(m_ecm, m_entity); - return utils::fromIgnitionVector(linkWorldAngAcc); + return utils::fromGzVector(linkWorldAngAcc); } std::array Link::bodyLinearAcceleration() const { const auto& linkBodyLinAcc = utils::getComponentData< - ignition::gazebo::components::LinearAcceleration>(m_ecm, m_entity); + gz::sim::components::LinearAcceleration>(m_ecm, m_entity); - return utils::fromIgnitionVector(linkBodyLinAcc); + return utils::fromGzVector(linkBodyLinAcc); } std::array Link::bodyAngularAcceleration() const { const auto& linkBodyAngAcc = utils::getComponentData< - ignition::gazebo::components::AngularAcceleration>(m_ecm, m_entity); + gz::sim::components::AngularAcceleration>(m_ecm, m_entity); - return utils::fromIgnitionVector(linkBodyAngAcc); + return utils::fromGzVector(linkBodyAngAcc); } bool Link::contactsEnabled() const { const auto& collisionEntities = m_ecm->ChildrenByComponents( m_entity, - ignition::gazebo::components::Collision(), - ignition::gazebo::components::ParentEntity(m_entity)); + gz::sim::components::Collision(), + gz::sim::components::ParentEntity(m_entity)); // If the link has no collision elements, we return true regardless. // To prevent surprises, e.g. users expecting that calling Link::inContact @@ -316,7 +316,7 @@ bool Link::contactsEnabled() const for (const auto collisionEntity : collisionEntities) { const bool hasContactSensorData = m_ecm->EntityHasComponentType( collisionEntity, - ignition::gazebo::components::ContactSensorData::typeId); + gz::sim::components::ContactSensorData::typeId); // Return false if a collision does not have the contact data component if (!hasContactSensorData) { @@ -334,15 +334,15 @@ bool Link::enableContactDetection(const bool enable) // Get all the collision entities of this link const auto& collisionEntities = m_ecm->ChildrenByComponents( m_entity, - ignition::gazebo::components::Collision(), - ignition::gazebo::components::ParentEntity(m_entity)); + gz::sim::components::Collision(), + gz::sim::components::ParentEntity(m_entity)); // Create the contact sensor data component that enables the Physics // system to extract contact information from the physics engine for (const auto collisionEntity : collisionEntities) { m_ecm->CreateComponent( collisionEntity, - ignition::gazebo::components::ContactSensorData()); + gz::sim::components::ContactSensorData()); } return true; @@ -352,8 +352,8 @@ bool Link::enableContactDetection(const bool enable) // Get all the collision entities of this link const auto& collisionEntities = m_ecm->ChildrenByComponents( m_entity, - ignition::gazebo::components::Collision(), - ignition::gazebo::components::ParentEntity(m_entity)); + gz::sim::components::Collision(), + gz::sim::components::ParentEntity(m_entity)); // Links with no collision elements already print a sDebug in the // contactsEnabled method, and not further action is needed @@ -364,7 +364,7 @@ bool Link::enableContactDetection(const bool enable) // Delete the contact sensor data component for (const auto collisionEntity : collisionEntities) { m_ecm->RemoveComponent< - ignition::gazebo::components::ContactSensorData>( + gz::sim::components::ContactSensorData>( collisionEntity); } @@ -388,8 +388,8 @@ std::vector Link::contacts() const { // Get the collisions of this link const auto& collisionEntities = m_ecm->EntitiesByComponents( - ignition::gazebo::components::ParentEntity(m_entity), - ignition::gazebo::components::Collision()); + gz::sim::components::ParentEntity(m_entity), + gz::sim::components::Collision()); // Return early if the link has no collision elements if (collisionEntities.empty()) { @@ -406,19 +406,19 @@ std::vector Link::contacts() const // Skip collisions entities without contact sensor if (!m_ecm->EntityHasComponentType( collisionEntity, - ignition::gazebo::components::ContactSensorData::typeId)) { + gz::sim::components::ContactSensorData::typeId)) { continue; } // Get the contact data for the selected collision entity - const ignition::msgs::Contacts& contactSensorData = + const gz::msgs::Contacts& contactSensorData = utils::getExistingComponentData< - ignition::gazebo::components::ContactSensorData>( + gz::sim::components::ContactSensorData>( m_ecm, collisionEntity); - // Convert the ignition msg + // Convert the gz msg const std::vector& collisionContacts = - utils::fromIgnitionContactsMsgs(m_ecm, contactSensorData); + utils::fromGzContactsMsgs(m_ecm, contactSensorData); for (const auto& contact : collisionContacts) { assert(!contact.bodyA.empty()); @@ -458,8 +458,8 @@ std::vector Link::contacts() const std::array Link::contactWrench() const { - auto totalForce = ignition::math::Vector3d::Zero; - auto totalTorque = ignition::math::Vector3d::Zero; + auto totalForce = gz::math::Vector3d::Zero; + auto totalTorque = gz::math::Vector3d::Zero; const auto& contacts = this->contacts(); @@ -474,10 +474,10 @@ std::array Link::contactWrench() const assert(contactPoint.torque == zero); // Link position - const auto& o_L = utils::toIgnitionVector3(this->position()); + const auto& o_L = utils::toGzVector3(this->position()); // Contact position - const auto& o_P = utils::toIgnitionVector3(contactPoint.position); + const auto& o_P = utils::toGzVector3(contactPoint.position); // Relative position const auto L_o_P = o_P - o_L; @@ -486,7 +486,7 @@ std::array Link::contactWrench() const // with the orientation of the world frame. This simplifies the // conversion since we have to take into account only the // displacement. - const auto& force = utils::toIgnitionVector3(contactPoint.force); + const auto& force = utils::toGzVector3(contactPoint.force); // The force does not have to be changed totalForce += force; @@ -520,29 +520,29 @@ bool Link::applyWorldWrench(const std::array& force, const std::array& torque, const double duration) { - // Adapted from ignition::gazebo::Link::AddWorld{Force,Wrench} + // Adapted from gz::sim::Link::AddWorld{Force,Wrench} // Initialize the force and the torque with the input data - const auto& forceIgnitionMath = utils::toIgnitionVector3(force); - const auto& torqueIgnitionMath = utils::toIgnitionVector3(torque); + const auto& forceGzMath = utils::toGzVector3(force); + const auto& torqueGzMath = utils::toGzVector3(torque); const auto entityWithSimTime = utils::getFirstParentEntityWithComponent< - ignition::gazebo::components::SimulatedTime>(m_ecm, m_entity); - assert(entityWithSimTime != ignition::gazebo::kNullEntity); + gz::sim::components::SimulatedTime>(m_ecm, m_entity); + assert(entityWithSimTime != gz::sim::kNullEntity); // Get the current simulated time const auto& now = utils::getExistingComponentData< - ignition::gazebo::components::SimulatedTime>(m_ecm, entityWithSimTime); + gz::sim::components::SimulatedTime>(m_ecm, entityWithSimTime); // Create a new wrench with duration const utils::WrenchWithDuration wrench( - forceIgnitionMath, - torqueIgnitionMath, + forceGzMath, + torqueGzMath, utils::doubleToSteadyClockDuration(duration), now); utils::LinkWrenchCmd& linkWrenchCmd = utils::getComponentData< - ignition::gazebo::components::ExternalWorldWrenchCmdWithDuration>( + gz::sim::components::ExternalWorldWrenchCmdWithDuration>( m_ecm, m_entity); linkWrenchCmd.addWorldWrench(wrench); @@ -553,11 +553,11 @@ bool Link::applyWorldWrenchToCoM(const std::array& force, const std::array& torque, const double duration) { - const ignition::math::Pose3d& worldPose = Impl::GetWorldPose(*this, *pImpl); + const gz::math::Pose3d& worldPose = Impl::GetWorldPose(*this, *pImpl); // Get the data of the inertial frame auto inertial = utils::getExistingComponentData< // - ignition::gazebo::components::Inertial>(m_ecm, m_entity); + gz::sim::components::Inertial>(m_ecm, m_entity); // We want the force to be applied at the center of mass, but // ExternalWorldWrenchCmd applies the force at the link origin so we need to @@ -568,13 +568,13 @@ bool Link::applyWorldWrenchToCoM(const std::array& force, worldPose.Rot().RotateVector(inertial.Pose().Pos()); // Initialize the force and the torque with the input data - const auto forceIgnitionMath = utils::toIgnitionVector3(force); - auto torqueIgnitionMath = utils::toIgnitionVector3(torque); + const auto forceGzMath = utils::toGzVector3(force); + auto torqueGzMath = utils::toGzVector3(torque); // Sum the component given by the projection of the force to the link origin - torqueIgnitionMath += linkCOMInWorldCoordinates.Cross(forceIgnitionMath); + torqueGzMath += linkCOMInWorldCoordinates.Cross(forceGzMath); - return this->applyWorldWrench(utils::fromIgnitionVector(forceIgnitionMath), - utils::fromIgnitionVector(torqueIgnitionMath), + return this->applyWorldWrench(utils::fromGzVector(forceGzMath), + utils::fromGzVector(torqueGzMath), duration); } diff --git a/scenario/src/gazebo/src/Model.cpp b/scenario/src/gazebo/src/Model.cpp index ce17f7432..e97ef18c2 100644 --- a/scenario/src/gazebo/src/Model.cpp +++ b/scenario/src/gazebo/src/Model.cpp @@ -38,21 +38,21 @@ #include "scenario/gazebo/helpers.h" #include "scenario/gazebo/utils.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -68,7 +68,7 @@ using namespace scenario::gazebo; class Model::Impl { public: - ignition::gazebo::Model model; + gz::sim::Model model; using LinkName = std::string; using JointName = std::string; @@ -118,11 +118,11 @@ uint64_t Model::id() const return std::hash{}(scopedModelName); } -bool Model::initialize(const ignition::gazebo::Entity modelEntity, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventManager) +bool Model::initialize(const gz::sim::Entity modelEntity, + gz::sim::EntityComponentManager* ecm, + gz::sim::EventManager* eventManager) { - if (modelEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { + if (modelEntity == gz::sim::kNullEntity || !ecm || !eventManager) { return false; } @@ -131,7 +131,7 @@ bool Model::initialize(const ignition::gazebo::Entity modelEntity, m_eventManager = eventManager; // Create the model - pImpl->model = ignition::gazebo::Model(modelEntity); + pImpl->model = gz::sim::Model(modelEntity); // Check that the model is valid if (!pImpl->model.Valid(*ecm)) { @@ -148,9 +148,9 @@ bool Model::createECMResources() // When the model is inserted, store the time of creation if (!m_ecm->EntityHasComponentType( - m_entity, ignition::gazebo::components::Timestamp::typeId)) { + m_entity, gz::sim::components::Timestamp::typeId)) { const auto& parentWorld = utils::getParentWorld(*this); - utils::setComponentData( + utils::setComponentData( m_ecm, m_entity, utils::doubleToSteadyClockDuration(parentWorld->time())); @@ -183,7 +183,7 @@ bool Model::createECMResources() // In this way controllers are never updated unless a new period is // configured. m_ecm->CreateComponent(m_entity, - ignition::gazebo::components::JointControllerPeriod( + gz::sim::components::JointControllerPeriod( std::chrono::steady_clock::duration().max())); return true; @@ -228,16 +228,16 @@ bool Model::resetBasePose(const std::array& position, { // Construct the desired transform between world and base const core::Pose pose(position, orientation); - ignition::math::Pose3d world_H_base = utils::toIgnitionPose(pose); + gz::math::Pose3d world_H_base = utils::toGzPose(pose); // Get the entity of the canonical link const auto canonicalLinkEntity = m_ecm->EntityByComponents( - ignition::gazebo::components::Link(), - ignition::gazebo::components::CanonicalLink(), - ignition::gazebo::components::Name(this->baseFrame()), - ignition::gazebo::components::ParentEntity(m_entity)); + gz::sim::components::Link(), + gz::sim::components::CanonicalLink(), + gz::sim::components::Name(this->baseFrame()), + gz::sim::components::ParentEntity(m_entity)); - if (canonicalLinkEntity == ignition::gazebo::kNullEntity) { + if (canonicalLinkEntity == gz::sim::kNullEntity) { sError << "Failed to get entity of canonical link" << std::endl; return false; } @@ -245,14 +245,14 @@ bool Model::resetBasePose(const std::array& position, // Get the Pose component of the canonical link. // This is the fixed transformation between the model and the base. auto& model_H_base = utils::getExistingComponentData< // - ignition::gazebo::components::Pose>(m_ecm, canonicalLinkEntity); + gz::sim::components::Pose>(m_ecm, canonicalLinkEntity); // Compute the robot pose that corresponds to the desired base pose - const ignition::math::Pose3d& world_H_model = + const gz::math::Pose3d& world_H_model = world_H_base * model_H_base.Inverse(); // Store the new pose - utils::setComponentData( + utils::setComponentData( m_ecm, m_entity, world_H_model); return true; @@ -279,30 +279,30 @@ bool Model::resetBaseWorldLinearVelocity(const std::array& linear) // Get the entity of the canonical (base) link const auto canonicalLinkEntity = m_ecm->EntityByComponents( - ignition::gazebo::components::Link(), - ignition::gazebo::components::CanonicalLink(), - ignition::gazebo::components::Name(this->baseFrame()), - ignition::gazebo::components::ParentEntity(m_entity)); + gz::sim::components::Link(), + gz::sim::components::CanonicalLink(), + gz::sim::components::Name(this->baseFrame()), + gz::sim::components::ParentEntity(m_entity)); // Get the Pose component of the canonical link. // This is the fixed transformation between the model and the base. const auto& M_H_B = utils::getExistingComponentData< // - ignition::gazebo::components::Pose>(m_ecm, canonicalLinkEntity); + gz::sim::components::Pose>(m_ecm, canonicalLinkEntity); // Get the rotation between base link and world - const auto& W_R_B = utils::toIgnitionQuaternion( + const auto& W_R_B = utils::toGzQuaternion( this->getLink(this->baseFrame())->orientation()); // Compute the linear part of the base link mixed velocity - const ignition::math::Vector3d baseLinearWorldVelocity = + const gz::math::Vector3d baseLinearWorldVelocity = utils::fromModelToBaseLinearVelocity( - utils::toIgnitionVector3(linear), - utils::toIgnitionVector3(this->baseWorldAngularVelocity()), + utils::toGzVector3(linear), + utils::toGzVector3(this->baseWorldAngularVelocity()), M_H_B, W_R_B); // Store the new velocity - utils::setComponentData( + utils::setComponentData( m_ecm, m_entity, baseLinearWorldVelocity); return true; @@ -314,8 +314,8 @@ bool Model::resetBaseWorldAngularVelocity(const std::array& angular) // link and the canonical link (as the linear part). // In fact, the angular velocity is invariant if there's a rigid // transformation between the two frames, like in this case. - utils::setComponentData( - m_ecm, m_entity, utils::toIgnitionVector3(angular)); + utils::setComponentData( + m_ecm, m_entity, utils::toGzVector3(angular)); return true; } @@ -384,7 +384,7 @@ scenario::core::LinkPtr Model::getLink(const std::string& linkName) const const auto linkEntity = pImpl->model.LinkByName(*m_ecm, linkName); - if (linkEntity == ignition::gazebo::kNullEntity) { + if (linkEntity == gz::sim::kNullEntity) { throw exceptions::LinkNotFound(linkName); } @@ -410,7 +410,7 @@ scenario::core::JointPtr Model::getJoint(const std::string& jointName) const const auto jointEntity = pImpl->model.JointByName(*m_ecm, jointName); - if (jointEntity == ignition::gazebo::kNullEntity) { + if (jointEntity == gz::sim::kNullEntity) { throw exceptions::JointNotFound(jointName); } @@ -439,13 +439,13 @@ std::vector Model::linkNames(const bool scoped) const std::vector linkNames; - m_ecm->Each( - [&](const ignition::gazebo::Entity& /*entity*/, - ignition::gazebo::components::Name* nameComponent, - ignition::gazebo::components::Link* /*linkComponent*/, - ignition::gazebo::components::ParentEntity* parentEntityComponent) + m_ecm->Each( + [&](const gz::sim::Entity& /*entity*/, + gz::sim::components::Name* nameComponent, + gz::sim::components::Link* /*linkComponent*/, + gz::sim::components::ParentEntity* parentEntityComponent) -> bool { assert(nameComponent); assert(parentEntityComponent); @@ -487,13 +487,13 @@ std::vector Model::jointNames(const bool scoped) const std::vector jointNames; - m_ecm->Each( - [&](const ignition::gazebo::Entity& /*jointEntity*/, - ignition::gazebo::components::Name* nameComponent, - ignition::gazebo::components::Joint* /*jointComponent*/, - ignition::gazebo::components::ParentEntity* parentEntityComponent) + m_ecm->Each( + [&](const gz::sim::Entity& /*jointEntity*/, + gz::sim::components::Name* nameComponent, + gz::sim::components::Joint* /*jointComponent*/, + gz::sim::components::ParentEntity* parentEntityComponent) -> bool { assert(nameComponent); assert(parentEntityComponent); @@ -532,7 +532,7 @@ std::vector Model::jointNames(const bool scoped) const double Model::controllerPeriod() const { const auto& duration = utils::getExistingComponentData< // - ignition::gazebo::components::JointControllerPeriod>(m_ecm, m_entity); + gz::sim::components::JointControllerPeriod>(m_ecm, m_entity); return utils::steadyClockDurationToDouble(duration); } @@ -547,7 +547,7 @@ bool Model::setControllerPeriod(const double period) // Store the new period in the ECM utils::setExistingComponentData< - ignition::gazebo::components::JointControllerPeriod>( + gz::sim::components::JointControllerPeriod>( m_ecm, m_entity, utils::doubleToSteadyClockDuration(period)); return true; } @@ -652,7 +652,7 @@ bool Model::enableContacts(const bool enable) bool Model::selfCollisionsEnabled() const { const bool selfCollisionsEnabled = utils::getExistingComponentData< - ignition::gazebo::components::SelfCollide>(m_ecm, m_entity); + gz::sim::components::SelfCollide>(m_ecm, m_entity); return selfCollisionsEnabled; } @@ -671,7 +671,7 @@ bool Model::enableSelfCollisions(const bool enable) return false; } - utils::setExistingComponentData( + utils::setExistingComponentData( m_ecm, m_entity, enable); return true; @@ -908,8 +908,8 @@ std::string Model::baseFrame() const { // Get all the canonical links of the model auto candidateBaseLinks = m_ecm->EntitiesByComponents( - ignition::gazebo::components::CanonicalLink(), - ignition::gazebo::components::ParentEntity(pImpl->model.Entity())); + gz::sim::components::CanonicalLink(), + gz::sim::components::ParentEntity(pImpl->model.Entity())); if (candidateBaseLinks.size() == 0) { throw exceptions::ModelError("Failed to find the canonical link", @@ -923,7 +923,7 @@ std::string Model::baseFrame() const // Get the name of the base link std::string baseLinkName = utils::getExistingComponentData< // - ignition::gazebo::components::Name>(m_ecm, candidateBaseLinks.front()); + gz::sim::components::Name>(m_ecm, candidateBaseLinks.front()); return baseLinkName; } @@ -931,48 +931,48 @@ std::string Model::baseFrame() const std::array Model::basePosition() const { // Get the model pose - const ignition::math::Pose3d& world_H_model = - utils::getExistingComponentData( + const gz::math::Pose3d& world_H_model = + utils::getExistingComponentData( m_ecm, m_entity); - return utils::fromIgnitionPose(world_H_model).position; + return utils::fromGzPose(world_H_model).position; } std::array Model::baseOrientation() const { // Get the model pose - const ignition::math::Pose3d& world_H_model = - utils::getExistingComponentData( + const gz::math::Pose3d& world_H_model = + utils::getExistingComponentData( m_ecm, m_entity); - return utils::fromIgnitionPose(world_H_model).orientation; + return utils::fromGzPose(world_H_model).orientation; } std::array Model::baseBodyLinearVelocity() const { const auto& baseWorldLinearVelocity = - utils::toIgnitionVector3(this->baseWorldLinearVelocity()); + utils::toGzVector3(this->baseWorldLinearVelocity()); // Get the model pose - const ignition::math::Pose3d& world_H_model = - utils::getExistingComponentData( + const gz::math::Pose3d& world_H_model = + utils::getExistingComponentData( m_ecm, m_entity); - return utils::fromIgnitionVector( // + return utils::fromGzVector( // world_H_model.Inverse().Rot() * baseWorldLinearVelocity); } std::array Model::baseBodyAngularVelocity() const { const auto& baseWorldAngularVelocity = - utils::toIgnitionVector3(this->baseWorldAngularVelocity()); + utils::toGzVector3(this->baseWorldAngularVelocity()); // Get the model pose - const ignition::math::Pose3d& world_H_model = - utils::getExistingComponentData( + const gz::math::Pose3d& world_H_model = + utils::getExistingComponentData( m_ecm, m_entity); - return utils::fromIgnitionVector( // + return utils::fromGzVector( // world_H_model.Inverse().Rot() * baseWorldAngularVelocity); } @@ -980,28 +980,28 @@ std::array Model::baseWorldLinearVelocity() const { // Get the entity of the canonical link const auto canonicalLinkEntity = m_ecm->EntityByComponents( - ignition::gazebo::components::Link(), - ignition::gazebo::components::CanonicalLink(), - ignition::gazebo::components::Name(this->baseFrame()), - ignition::gazebo::components::ParentEntity(m_entity)); + gz::sim::components::Link(), + gz::sim::components::CanonicalLink(), + gz::sim::components::Name(this->baseFrame()), + gz::sim::components::ParentEntity(m_entity)); // Get the Pose component of the canonical link. // This is the fixed transformation between the model and the base. const auto& M_H_B = utils::getExistingComponentData< // - ignition::gazebo::components::Pose>(m_ecm, canonicalLinkEntity); + gz::sim::components::Pose>(m_ecm, canonicalLinkEntity); // Get the rotation between base link and world - const auto& W_R_B = utils::toIgnitionQuaternion( + const auto& W_R_B = utils::toGzQuaternion( this->getLink(this->baseFrame())->orientation()); // Get the linear velocity of the canonical link - const ignition::math::Vector3d& canonicalLinkLinearVelocity = - utils::toIgnitionVector3( + const gz::math::Vector3d& canonicalLinkLinearVelocity = + utils::toGzVector3( this->getLink(this->baseFrame())->worldLinearVelocity()); // Get the angular velocity of the canonical link - const ignition::math::Vector3d& canonicalLinkAngularVelocity = - utils::toIgnitionVector3( + const gz::math::Vector3d& canonicalLinkAngularVelocity = + utils::toGzVector3( this->getLink(this->baseFrame())->worldAngularVelocity()); // Convert the base velocity to the model mixed velocity @@ -1012,7 +1012,7 @@ std::array Model::baseWorldLinearVelocity() const W_R_B); // Return the linear part - return utils::fromIgnitionVector(modelLinearVelocity); + return utils::fromGzVector(modelLinearVelocity); } std::array Model::baseWorldAngularVelocity() const @@ -1032,10 +1032,10 @@ std::array Model::baseWorldAngularVelocity() const bool Model::setBasePoseTarget(const std::array& position, const std::array& orientation) { - const ignition::math::Pose3d& basePoseTarget = - utils::toIgnitionPose(core::Pose{position, orientation}); + const gz::math::Pose3d& basePoseTarget = + utils::toGzPose(core::Pose{position, orientation}); - utils::setComponentData( + utils::setComponentData( m_ecm, m_entity, basePoseTarget); return true; @@ -1044,15 +1044,15 @@ bool Model::setBasePoseTarget(const std::array& position, bool Model::setBasePositionTarget(const std::array& position) { const auto& basePoseTargetComponent = utils::getComponent< // - ignition::gazebo::components::BasePoseTarget>( - m_ecm, m_entity, ignition::math::Pose3d::Zero); + gz::sim::components::BasePoseTarget>( + m_ecm, m_entity, gz::math::Pose3d::Zero); const auto basePoseTarget = - ignition::math::Pose3d(utils::toIgnitionVector3(position), + gz::math::Pose3d(utils::toGzVector3(position), basePoseTargetComponent->Data().Rot()); utils::setExistingComponentData< - ignition::gazebo::components::BasePoseTarget>( + gz::sim::components::BasePoseTarget>( m_ecm, m_entity, basePoseTarget); return true; @@ -1061,14 +1061,14 @@ bool Model::setBasePositionTarget(const std::array& position) bool Model::setBaseOrientationTarget(const std::array& orientation) { const auto& basePoseTargetComponent = utils::getComponent< // - ignition::gazebo::components::BasePoseTarget, - ignition::math::Pose3d>(m_ecm, m_entity); + gz::sim::components::BasePoseTarget, + gz::math::Pose3d>(m_ecm, m_entity); const auto basePoseTarget = - ignition::math::Pose3d(basePoseTargetComponent->Data().Pos(), - utils::toIgnitionQuaternion(orientation)); + gz::math::Pose3d(basePoseTargetComponent->Data().Pos(), + utils::toGzQuaternion(orientation)); - utils::setComponentData( + utils::setComponentData( m_ecm, m_entity, basePoseTarget); return true; @@ -1086,11 +1086,11 @@ bool Model::setBaseWorldVelocityTarget(const std::array& linear, bool Model::setBaseWorldLinearVelocityTarget( const std::array& linear) { - const ignition::math::Vector3d& baseWorldLinearVelocity = - utils::toIgnitionVector3(linear); + const gz::math::Vector3d& baseWorldLinearVelocity = + utils::toGzVector3(linear); utils::setComponentData< - ignition::gazebo::components::BaseWorldLinearVelocityTarget>( + gz::sim::components::BaseWorldLinearVelocityTarget>( m_ecm, m_entity, baseWorldLinearVelocity); return true; @@ -1099,11 +1099,11 @@ bool Model::setBaseWorldLinearVelocityTarget( bool Model::setBaseWorldAngularVelocityTarget( const std::array& angular) { - const ignition::math::Vector3d& baseWorldAngularVelocity = - utils::toIgnitionVector3(angular); + const gz::math::Vector3d& baseWorldAngularVelocity = + utils::toGzVector3(angular); utils::setComponentData< - ignition::gazebo::components::BaseWorldAngularVelocityTarget>( + gz::sim::components::BaseWorldAngularVelocityTarget>( m_ecm, m_entity, baseWorldAngularVelocity); return true; @@ -1112,15 +1112,15 @@ bool Model::setBaseWorldAngularVelocityTarget( bool Model::setBaseWorldLinearAccelerationTarget( const std::array& linear) { - const ignition::math::Vector3d& baseWorldLinearAcceleration = - utils::toIgnitionVector3(linear); + const gz::math::Vector3d& baseWorldLinearAcceleration = + utils::toGzVector3(linear); // TODO: do we need to convert the model acceleration to base link // acceleration? Contrarily to the velocity, this component // is not used in gazebo but from custom controllers. utils::setComponentData< - ignition::gazebo::components::BaseWorldLinearAccelerationTarget>( + gz::sim::components::BaseWorldLinearAccelerationTarget>( m_ecm, m_entity, baseWorldLinearAcceleration); return true; @@ -1129,11 +1129,11 @@ bool Model::setBaseWorldLinearAccelerationTarget( bool Model::setBaseWorldAngularAccelerationTarget( const std::array& angular) { - const ignition::math::Vector3d& baseWorldAngularAcceleration = - utils::toIgnitionVector3(angular); + const gz::math::Vector3d& baseWorldAngularAcceleration = + utils::toGzVector3(angular); utils::setComponentData< - ignition::gazebo::components::BaseWorldAngularAccelerationTarget>( + gz::sim::components::BaseWorldAngularAccelerationTarget>( m_ecm, m_entity, baseWorldAngularAcceleration); return true; @@ -1141,60 +1141,60 @@ bool Model::setBaseWorldAngularAccelerationTarget( std::array Model::basePositionTarget() const { - const ignition::math::Pose3d& basePoseTarget = + const gz::math::Pose3d& basePoseTarget = utils::getExistingComponentData< - ignition::gazebo::components::BasePoseTarget>(m_ecm, m_entity); + gz::sim::components::BasePoseTarget>(m_ecm, m_entity); - return utils::fromIgnitionPose(basePoseTarget).position; + return utils::fromGzPose(basePoseTarget).position; } std::array Model::baseOrientationTarget() const { - const ignition::math::Pose3d& basePoseTarget = + const gz::math::Pose3d& basePoseTarget = utils::getExistingComponentData< - ignition::gazebo::components::BasePoseTarget>(m_ecm, m_entity); + gz::sim::components::BasePoseTarget>(m_ecm, m_entity); - return utils::fromIgnitionPose(basePoseTarget).orientation; + return utils::fromGzPose(basePoseTarget).orientation; } std::array Model::baseWorldLinearVelocityTarget() const { - const ignition::math::Vector3d& baseLinTarget = + const gz::math::Vector3d& baseLinTarget = utils::getExistingComponentData< - ignition::gazebo::components::BaseWorldLinearVelocityTarget>( + gz::sim::components::BaseWorldLinearVelocityTarget>( m_ecm, m_entity); - return utils::fromIgnitionVector(baseLinTarget); + return utils::fromGzVector(baseLinTarget); } std::array Model::baseWorldAngularVelocityTarget() const { - const ignition::math::Vector3d& baseAngTarget = + const gz::math::Vector3d& baseAngTarget = utils::getExistingComponentData< - ignition::gazebo::components::BaseWorldAngularVelocityTarget>( + gz::sim::components::BaseWorldAngularVelocityTarget>( m_ecm, m_entity); - return utils::fromIgnitionVector(baseAngTarget); + return utils::fromGzVector(baseAngTarget); } std::array Model::baseWorldLinearAccelerationTarget() const { - const ignition::math::Vector3d& baseLinTarget = + const gz::math::Vector3d& baseLinTarget = utils::getExistingComponentData< - ignition::gazebo::components::BaseWorldLinearAccelerationTarget>( + gz::sim::components::BaseWorldLinearAccelerationTarget>( m_ecm, m_entity); - return utils::fromIgnitionVector(baseLinTarget); + return utils::fromGzVector(baseLinTarget); } std::array Model::baseWorldAngularAccelerationTarget() const { - const ignition::math::Vector3d& baseAngTarget = + const gz::math::Vector3d& baseAngTarget = utils::getExistingComponentData< - ignition::gazebo::components::BaseWorldAngularAccelerationTarget>( + gz::sim::components::BaseWorldAngularAccelerationTarget>( m_ecm, m_entity); - return utils::fromIgnitionVector(baseAngTarget); + return utils::fromGzVector(baseAngTarget); } // ====================== diff --git a/scenario/src/gazebo/src/World.cpp b/scenario/src/gazebo/src/World.cpp index bbdc63ee4..0b682e075 100644 --- a/scenario/src/gazebo/src/World.cpp +++ b/scenario/src/gazebo/src/World.cpp @@ -33,19 +33,19 @@ #include "scenario/gazebo/helpers.h" #include "scenario/gazebo/utils.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -60,7 +60,7 @@ using namespace scenario::gazebo; class World::Impl { public: - std::shared_ptr sdfEntityCreator; + std::shared_ptr sdfEntityCreator; using ModelName = std::string; std::unordered_map models; @@ -123,7 +123,7 @@ class World::Impl } // Create the model entity - const ignition::gazebo::Entity modelEntity = + const gz::sim::Entity modelEntity = this->sdfEntityCreator->CreateEntities(modelSdfRoot->Model()); // Attach the model entity to the world entity @@ -133,7 +133,7 @@ class World::Impl // Check that the model name is correct std::string modelNameSDF = modelSdfRoot->Model()->Name(); std::string modelNameEntity = utils::getExistingComponentData< // - ignition::gazebo::components::Name>(world.m_ecm, modelEntity); + gz::sim::components::Name>(world.m_ecm, modelEntity); assert(modelNameSDF == modelNameEntity); } @@ -164,8 +164,8 @@ class World::Impl // needs to be processed by the Physics system. Overriding the // component, instead, has instantaneous effect. if (pose != core::Pose::Identity()) { - utils::setComponentData( - world.m_ecm, modelEntity, utils::toIgnitionPose(pose)); + utils::setComponentData( + world.m_ecm, modelEntity, utils::toGzPose(pose)); } return true; @@ -183,11 +183,11 @@ uint64_t World::id() const return std::hash{}(this->name()); } -bool World::initialize(const ignition::gazebo::Entity worldEntity, - ignition::gazebo::EntityComponentManager* ecm, - ignition::gazebo::EventManager* eventManager) +bool World::initialize(const gz::sim::Entity worldEntity, + gz::sim::EntityComponentManager* ecm, + gz::sim::EventManager* eventManager) { - if (worldEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { + if (worldEntity == gz::sim::kNullEntity || !ecm || !eventManager) { return false; } @@ -198,7 +198,7 @@ bool World::initialize(const ignition::gazebo::Entity worldEntity, // Create the SdfEntityCreator pImpl->sdfEntityCreator = std::make_unique< // - ignition::gazebo::SdfEntityCreator>(*ecm, *eventManager); + gz::sim::SdfEntityCreator>(*ecm, *eventManager); return true; } @@ -207,21 +207,21 @@ bool World::createECMResources() { // Store the time of creation (big bang) if (!m_ecm->EntityHasComponentType( - m_entity, ignition::gazebo::components::Timestamp::typeId)) { - utils::setComponentData( + m_entity, gz::sim::components::Timestamp::typeId)) { + utils::setComponentData( m_ecm, m_entity, std::chrono::steady_clock::duration::zero()); } // Initialize the simulated time at 0.0 (Physics will then update it) if (!m_ecm->EntityHasComponentType( - m_entity, ignition::gazebo::components::SimulatedTime::typeId)) { - utils::setComponentData( + m_entity, gz::sim::components::SimulatedTime::typeId)) { + utils::setComponentData( m_ecm, m_entity, std::chrono::steady_clock::duration::zero()); } // Print the active physics profile const auto& physics = utils::getExistingComponentData< // - ignition::gazebo::components::Physics>(m_ecm, m_entity); + gz::sim::components::Physics>(m_ecm, m_entity); sDebug << "Initializing world '" << this->name() << "' with physics parameters:" << std::endl << "rtf=" << physics.RealTimeFactor() << std::endl @@ -254,8 +254,8 @@ bool World::setPhysicsEngine(const PhysicsEngine engine) const std::string pluginLib = [&engine]() -> std::string { switch (engine) { case PhysicsEngine::Dart: - return "ignition-physics" - + std::to_string(IGNITION_PHYSICS_MAJOR_VERSION) + return "gz-physics" + + std::to_string(GZ_PHYSICS_MAJOR_VERSION) + "-dartsim-plugin"; } return ""; @@ -267,7 +267,7 @@ bool World::setPhysicsEngine(const PhysicsEngine engine) } // This component is read by the Physics system during its configuration - utils::setComponentData( + utils::setComponentData( m_ecm, m_entity, pluginLib); // Vendored Physics system @@ -286,15 +286,15 @@ bool World::setPhysicsEngine(const PhysicsEngine engine) std::array World::gravity() const { const auto& gravity = utils::getExistingComponentData< // - ignition::gazebo::components::Gravity>(m_ecm, m_entity); + gz::sim::components::Gravity>(m_ecm, m_entity); - return utils::fromIgnitionVector(gravity); + return utils::fromGzVector(gravity); } bool World::setGravity(const std::array& gravity) { const auto& simTimeAtWorldCreation = utils::getExistingComponentData< - ignition::gazebo::components::Timestamp>(m_ecm, m_entity); + gz::sim::components::Timestamp>(m_ecm, m_entity); const double simTimeAtWorldCreationInSeconds = utils::steadyClockDurationToDouble(simTimeAtWorldCreation); @@ -305,8 +305,8 @@ bool World::setGravity(const std::array& gravity) return false; } - utils::setExistingComponentData( - m_ecm, m_entity, utils::toIgnitionVector3(gravity)); + utils::setExistingComponentData( + m_ecm, m_entity, utils::toGzVector3(gravity)); return true; } @@ -319,7 +319,7 @@ bool World::valid() const double World::time() const { const auto& simTime = utils::getExistingComponentData< - ignition::gazebo::components::SimulatedTime>(m_ecm, m_entity); + gz::sim::components::SimulatedTime>(m_ecm, m_entity); return utils::steadyClockDurationToDouble(simTime); } @@ -327,7 +327,7 @@ double World::time() const std::string World::name() const { const std::string& worldName = utils::getExistingComponentData< // - ignition::gazebo::components::Name>(m_ecm, m_entity); + gz::sim::components::Name>(m_ecm, m_entity); return worldName; } @@ -336,13 +336,13 @@ std::vector World::modelNames() const { pImpl->buffers.modelNames.clear(); - m_ecm->Each( - [&](const ignition::gazebo::Entity& /*entity*/, - ignition::gazebo::components::Name* nameComponent, - ignition::gazebo::components::Model* /*modelComponent*/, - ignition::gazebo::components::ParentEntity* parentEntityComponent) + m_ecm->Each( + [&](const gz::sim::Entity& /*entity*/, + gz::sim::components::Name* nameComponent, + gz::sim::components::Model* /*modelComponent*/, + gz::sim::components::ParentEntity* parentEntityComponent) -> bool { assert(nameComponent); assert(parentEntityComponent); @@ -368,11 +368,11 @@ scenario::core::ModelPtr World::getModel(const std::string& modelName) const // Find the model entity const auto modelEntity = m_ecm->EntityByComponents( - ignition::gazebo::components::Name(modelName), - ignition::gazebo::components::Model(), - ignition::gazebo::components::ParentEntity(m_entity)); + gz::sim::components::Name(modelName), + gz::sim::components::Model(), + gz::sim::components::ParentEntity(m_entity)); - if (modelEntity == ignition::gazebo::kNullEntity) { + if (modelEntity == gz::sim::kNullEntity) { throw exceptions::ModelNotFound(modelName); } @@ -438,11 +438,11 @@ bool World::insertModelFromString(const std::string& sdfString, bool World::removeModel(const std::string& modelName) { const auto modelEntity = m_ecm->EntityByComponents( - ignition::gazebo::components::Name(modelName), - ignition::gazebo::components::Model(), - ignition::gazebo::components::ParentEntity(m_entity)); + gz::sim::components::Name(modelName), + gz::sim::components::Model(), + gz::sim::components::ParentEntity(m_entity)); - if (modelEntity == ignition::gazebo::kNullEntity) { + if (modelEntity == gz::sim::kNullEntity) { sError << "Model '" << modelName << "' not found in the world" << std::endl; return false; diff --git a/scenario/src/gazebo/src/helpers.cpp b/scenario/src/gazebo/src/helpers.cpp index 843237151..b0172e4fe 100644 --- a/scenario/src/gazebo/src/helpers.cpp +++ b/scenario/src/gazebo/src/helpers.cpp @@ -25,16 +25,16 @@ */ #include "scenario/gazebo/helpers.h" -#include "ignition/common/Util.hh" +#include "gz/common/Util.hh" #include "scenario/gazebo/Log.h" #include "scenario/gazebo/components/Timestamp.h" #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include @@ -90,7 +90,7 @@ utils::getSdfRootFromString(const std::string& sdfString) bool utils::verboseFromEnvironment() { std::string envVarContent; - ignition::common::env(ScenarioVerboseEnvVar, envVarContent); + gz::common::env(ScenarioVerboseEnvVar, envVarContent); return envVarContent == "1"; } @@ -134,20 +134,20 @@ bool utils::parentModelJustCreated(const GazeboEntity& gazeboEntity) const auto& world = utils::getParentWorld(gazeboEntity); // Get the entity of the parent model - const auto parentModelEntity = [&]() -> ignition::gazebo::Entity { + const auto parentModelEntity = [&]() -> gz::sim::Entity { return gazeboEntity.ecm()->EntityHasComponentType( gazeboEntity.entity(), - ignition::gazebo::components::Model::typeId) + gz::sim::components::Model::typeId) ? gazeboEntity.entity() : utils::getParentModel(gazeboEntity)->entity(); }(); assert(world); - assert(parentModelEntity != ignition::gazebo::kNullEntity); + assert(parentModelEntity != gz::sim::kNullEntity); // Get the time the model was inserted const auto& simTimeAtModelCreation = utils::getExistingComponentData< - ignition::gazebo::components::Timestamp>(gazeboEntity.ecm(), + gz::sim::components::Timestamp>(gazeboEntity.ecm(), parentModelEntity); const double simTimeAtModelCreationInSeconds = @@ -157,45 +157,45 @@ bool utils::parentModelJustCreated(const GazeboEntity& gazeboEntity) } scenario::core::Pose -utils::fromIgnitionPose(const ignition::math::Pose3d& ignitionPose) +utils::fromGzPose(const gz::math::Pose3d& gzPose) { core::Pose pose; - pose.position[0] = ignitionPose.Pos().X(); - pose.position[1] = ignitionPose.Pos().Y(); - pose.position[2] = ignitionPose.Pos().Z(); + pose.position[0] = gzPose.Pos().X(); + pose.position[1] = gzPose.Pos().Y(); + pose.position[2] = gzPose.Pos().Z(); - pose.orientation[0] = ignitionPose.Rot().W(); - pose.orientation[1] = ignitionPose.Rot().X(); - pose.orientation[2] = ignitionPose.Rot().Y(); - pose.orientation[3] = ignitionPose.Rot().Z(); + pose.orientation[0] = gzPose.Rot().W(); + pose.orientation[1] = gzPose.Rot().X(); + pose.orientation[2] = gzPose.Rot().Y(); + pose.orientation[3] = gzPose.Rot().Z(); return pose; } -ignition::math::Pose3d utils::toIgnitionPose(const scenario::core::Pose& pose) +gz::math::Pose3d utils::toGzPose(const scenario::core::Pose& pose) { - ignition::math::Pose3d ignitionPose; + gz::math::Pose3d gzPose; - ignitionPose.Pos() = ignition::math::Vector3d( + gzPose.Pos() = gz::math::Vector3d( pose.position[0], pose.position[1], pose.position[2]); - ignitionPose.Rot() = ignition::math::Quaterniond(pose.orientation[0], + gzPose.Rot() = gz::math::Quaterniond(pose.orientation[0], pose.orientation[1], pose.orientation[2], pose.orientation[3]); - return ignitionPose; + return gzPose; } scenario::core::Contact -utils::fromIgnitionContactMsgs(ignition::gazebo::EntityComponentManager* ecm, - const ignition::msgs::Contact& contactMsg) +utils::fromGzContactMsgs(gz::sim::EntityComponentManager* ecm, + const gz::msgs::Contact& contactMsg) { auto getEntityName = - [&](const ignition::gazebo::Entity entity) -> std::string { + [&](const gz::sim::Entity entity) -> std::string { return utils::getExistingComponentData< - ignition::gazebo::components::Name>(ecm, entity); + gz::sim::components::Name>(ecm, entity); }; // Get the names of the links in contact following: @@ -233,8 +233,8 @@ utils::fromIgnitionContactMsgs(ignition::gazebo::EntityComponentManager* ecm, assert(numOfPoints == numOfWrenches); assert(numOfPoints == numOfPositions); - auto fromIgnMsg = - [](const ignition::msgs::Vector3d& vec) -> std::array { + auto fromGzmsg = + [](const gz::msgs::Vector3d& vec) -> std::array { return {vec.x(), vec.y(), vec.z()}; }; @@ -243,14 +243,14 @@ utils::fromIgnitionContactMsgs(ignition::gazebo::EntityComponentManager* ecm, // Create a contact point scenario::core::ContactPoint contactPoint; contactPoint.depth = contactMsg.depth(pointIdx); - contactPoint.normal = fromIgnMsg(contactMsg.normal(pointIdx)); - contactPoint.position = fromIgnMsg(contactMsg.position(pointIdx)); + contactPoint.normal = fromGzmsg(contactMsg.normal(pointIdx)); + contactPoint.position = fromGzmsg(contactMsg.position(pointIdx)); // Get the wrench acting on bodyA - const ignition::msgs::JointWrench wrench = contactMsg.wrench(pointIdx); + const gz::msgs::JointWrench wrench = contactMsg.wrench(pointIdx); const auto& wrench1 = wrench.body_1_wrench(); - contactPoint.force = fromIgnMsg(wrench1.force()); - contactPoint.torque = fromIgnMsg(wrench1.torque()); + contactPoint.force = fromGzmsg(wrench1.force()); + contactPoint.torque = fromGzmsg(wrench1.torque()); // Store the contact point contact.points.push_back(contactPoint); @@ -260,15 +260,15 @@ utils::fromIgnitionContactMsgs(ignition::gazebo::EntityComponentManager* ecm, } std::vector -utils::fromIgnitionContactsMsgs(ignition::gazebo::EntityComponentManager* ecm, - const ignition::msgs::Contacts& contactsMsg) +utils::fromGzContactsMsgs(gz::sim::EntityComponentManager* ecm, + const gz::msgs::Contacts& contactsMsg) { std::vector contacts; for (int contactIdx = 0; contactIdx < contactsMsg.contact_size(); ++contactIdx) { contacts.push_back( - fromIgnitionContactMsgs(ecm, contactsMsg.contact(contactIdx))); + fromGzContactMsgs(ecm, contactsMsg.contact(contactIdx))); } return contacts; @@ -499,11 +499,11 @@ scenario::core::JointType utils::fromSdf(const sdf::JointType sdfType) return type; } -ignition::math::Vector3d utils::fromModelToBaseLinearVelocity( - const ignition::math::Vector3d& linModelVelocity, - const ignition::math::Vector3d& angModelVelocity, - const ignition::math::Pose3d& M_H_B, - const ignition::math::Quaterniond& W_R_B) +gz::math::Vector3d utils::fromModelToBaseLinearVelocity( + const gz::math::Vector3d& linModelVelocity, + const gz::math::Vector3d& angModelVelocity, + const gz::math::Pose3d& M_H_B, + const gz::math::Quaterniond& W_R_B) { // Extract the rotation and the position of the model wrt to the base auto B_R_M = M_H_B.Rot().Inverse(); @@ -511,25 +511,25 @@ ignition::math::Vector3d utils::fromModelToBaseLinearVelocity( auto B_o_M = -B_R_M * M_o_B; // Compute the base linear velocity - const ignition::math::Vector3d linBaseVelocity = + const gz::math::Vector3d linBaseVelocity = linModelVelocity - angModelVelocity.Cross(W_R_B * B_o_M); // Return the linear part of the mixed velocity of the base return linBaseVelocity; } -ignition::math::Vector3d utils::fromBaseToModelLinearVelocity( - const ignition::math::Vector3d& linBaseVelocity, - const ignition::math::Vector3d& angBaseVelocity, - const ignition::math::Pose3d& M_H_B, - const ignition::math::Quaterniond& W_R_B) +gz::math::Vector3d utils::fromBaseToModelLinearVelocity( + const gz::math::Vector3d& linBaseVelocity, + const gz::math::Vector3d& angBaseVelocity, + const gz::math::Pose3d& M_H_B, + const gz::math::Quaterniond& W_R_B) { // Extract the rotation and the position of the model wrt to the base auto B_R_M = M_H_B.Rot().Inverse(); auto M_o_B = M_H_B.Pos(); // Compute the model linear velocity - const ignition::math::Vector3d linModelVelocity = + const gz::math::Vector3d linModelVelocity = linBaseVelocity - angBaseVelocity.Cross(W_R_B * B_R_M * M_o_B); // Return the linear part of the mixed velocity of the model @@ -544,10 +544,10 @@ std::shared_ptr utils::getParentWorld(const GazeboEntity& gazeboEntity) } auto worldEntity = getFirstParentEntityWithComponent< // - ignition::gazebo::components::World>(gazeboEntity.ecm(), + gz::sim::components::World>(gazeboEntity.ecm(), gazeboEntity.entity()); - if (worldEntity == ignition::gazebo::kNullEntity) { + if (worldEntity == gz::sim::kNullEntity) { sError << "Failed to find parent world entity" << std::endl; return nullptr; } @@ -571,10 +571,10 @@ std::shared_ptr utils::getParentModel(const GazeboEntity& gazeboEntity) } auto modelEntity = getFirstParentEntityWithComponent< // - ignition::gazebo::components::Model>(gazeboEntity.ecm(), + gz::sim::components::Model>(gazeboEntity.ecm(), gazeboEntity.entity()); - if (modelEntity == ignition::gazebo::kNullEntity) { + if (modelEntity == gz::sim::kNullEntity) { sError << "Failed to find parent model entity" << std::endl; return nullptr; } diff --git a/scenario/src/gazebo/src/utils.cpp b/scenario/src/gazebo/src/utils.cpp index 2405ed12d..9a031dc10 100644 --- a/scenario/src/gazebo/src/utils.cpp +++ b/scenario/src/gazebo/src/utils.cpp @@ -29,16 +29,16 @@ #include "scenario/gazebo/helpers.h" #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -52,7 +52,7 @@ using namespace scenario::gazebo; void utils::setVerbosity(const Verbosity level) { - ignition::common::Console::SetVerbosity(static_cast(level)); + gz::common::Console::SetVerbosity(static_cast(level)); } std::string utils::findSdfFile(const std::string& fileName) @@ -62,16 +62,16 @@ std::string utils::findSdfFile(const std::string& fileName) return {}; } - ignition::common::SystemPaths systemPaths; - systemPaths.SetFilePathEnv("IGN_GAZEBO_RESOURCE_PATH"); - systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + gz::common::SystemPaths systemPaths; + systemPaths.SetFilePathEnv("GZ_SIM_RESOURCE_PATH"); + systemPaths.AddFilePaths(GZ_SIM_WORLD_INSTALL_DIR); // Find the file std::string sdfFilePath = systemPaths.FindFile(fileName); if (sdfFilePath.empty()) { sError << "Failed to find " << fileName << std::endl; - sError << "Check that it is part of IGN_GAZEBO_RESOURCE_PATH" + sError << "Check that it is part of GZ_SIM_RESOURCE_PATH" << std::endl; return {}; } @@ -90,7 +90,7 @@ std::string utils::getSdfString(const std::string& fileName) // support is still rough even with C++17 enabled :/ std::string sdfFileAbsPath; - if (!ignition::common::isFile(fileName)) { + if (!gz::common::isFile(fileName)) { sdfFileAbsPath = findSdfFile(fileName); } @@ -193,7 +193,7 @@ std::string utils::getModelFileFromFuel(const std::string& URI, const bool useCache) { std::string modelFilePath; - using namespace ignition; + using namespace gz; if (!useCache) { modelFilePath = fuel_tools::fetchResource(URI); @@ -419,7 +419,7 @@ bool utils::insertPluginToGazeboEntity(const GazeboEntity& gazeboEntity, const auto wrapped = sdf::SDF::WrapInRoot(pluginElement); // Trigger the plugin loading - gazeboEntity.eventManager()->Emit( + gazeboEntity.eventManager()->Emit( gazeboEntity.entity(), wrapped); return true; diff --git a/scenario/src/plugins/ControllerRunner/CMakeLists.txt b/scenario/src/plugins/ControllerRunner/CMakeLists.txt index c32449681..2b13678ff 100644 --- a/scenario/src/plugins/ControllerRunner/CMakeLists.txt +++ b/scenario/src/plugins/ControllerRunner/CMakeLists.txt @@ -52,7 +52,7 @@ add_library(ControllerRunner SHARED target_link_libraries(ControllerRunner PUBLIC - ${ignition-gazebo.core} + ${gz-sim.core} PRIVATE ScenarioGazebo::ScenarioGazebo ScenarioGazebo::ExtraComponents diff --git a/scenario/src/plugins/ControllerRunner/ControllerRunner.cpp b/scenario/src/plugins/ControllerRunner/ControllerRunner.cpp index c2b0e93f3..f3ce76eb2 100644 --- a/scenario/src/plugins/ControllerRunner/ControllerRunner.cpp +++ b/scenario/src/plugins/ControllerRunner/ControllerRunner.cpp @@ -37,10 +37,10 @@ #include "scenario/gazebo/exceptions.h" #include "scenario/gazebo/helpers.h" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -60,7 +60,7 @@ class ControllerRunner::Impl bool referencesHaveBeenSet = false; std::shared_ptr model; - ignition::gazebo::Entity modelEntity; + gz::sim::Entity modelEntity; std::chrono::steady_clock::duration prevUpdateTime{0}; std::shared_ptr controller; @@ -76,13 +76,13 @@ class ControllerRunner::Impl } controllerInterfaces; bool - updateAllSupportedReferences(ignition::gazebo::EntityComponentManager& ecm); + updateAllSupportedReferences(gz::sim::EntityComponentManager& ecm); bool - updateBaseReferencesfromECM(ignition::gazebo::EntityComponentManager& ecm); + updateBaseReferencesfromECM(gz::sim::EntityComponentManager& ecm); bool - updateJointReferencesfromECM(ignition::gazebo::EntityComponentManager& ecm); + updateJointReferencesfromECM(gz::sim::EntityComponentManager& ecm); void printControllerContext( const std::shared_ptr context) const; @@ -99,10 +99,10 @@ ControllerRunner::ControllerRunner() // and there's no more ECM -> we would get segfault. ControllerRunner::~ControllerRunner() = default; -void ControllerRunner::Configure(const ignition::gazebo::Entity& entity, +void ControllerRunner::Configure(const gz::sim::Entity& entity, const std::shared_ptr& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& eventMgr) { // Store the model entity @@ -180,8 +180,8 @@ void ControllerRunner::Configure(const ignition::gazebo::Entity& entity, sDebug << "Controller successfully initialized" << std::endl; } -void ControllerRunner::PreUpdate(const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& ecm) +void ControllerRunner::PreUpdate(const gz::sim::UpdateInfo& info, + gz::sim::EntityComponentManager& ecm) { if (info.paused) { return; @@ -282,7 +282,7 @@ void ControllerRunner::PreUpdate(const ignition::gazebo::UpdateInfo& info, } bool ControllerRunner::Impl::updateAllSupportedReferences( - ignition::gazebo::EntityComponentManager& ecm) + gz::sim::EntityComponentManager& ecm) { bool ok = true; @@ -317,11 +317,11 @@ bool ControllerRunner::Impl::updateAllSupportedReferences( } bool ControllerRunner::Impl::updateBaseReferencesfromECM( - ignition::gazebo::EntityComponentManager& ecm) + gz::sim::EntityComponentManager& ecm) { assert(controllerInterfaces.base); - using namespace ignition::math; - using namespace ignition::gazebo; + using namespace gz::math; + using namespace gz::sim; // ========= // Base Pose @@ -330,7 +330,7 @@ bool ControllerRunner::Impl::updateBaseReferencesfromECM( Pose3d& basePoseTarget = utils::getExistingComponentData< // components::BasePoseTarget>(&ecm, modelEntity); - core::Pose basePose = utils::fromIgnitionPose(basePoseTarget); + core::Pose basePose = utils::fromGzPose(basePoseTarget); baseReferences.position = basePose.position; baseReferences.orientation = basePose.orientation; @@ -346,9 +346,9 @@ bool ControllerRunner::Impl::updateBaseReferencesfromECM( components::BaseWorldAngularVelocityTarget>(&ecm, modelEntity); baseReferences.linearVelocity = - utils::fromIgnitionVector(baseLinearVelocityTarget); + utils::fromGzVector(baseLinearVelocityTarget); baseReferences.angularVelocity = - utils::fromIgnitionVector(baseAngularVelocityTarget); + utils::fromGzVector(baseAngularVelocityTarget); // ================= // Base Acceleration @@ -361,15 +361,15 @@ bool ControllerRunner::Impl::updateBaseReferencesfromECM( components::BaseWorldAngularAccelerationTarget>(&ecm, modelEntity); baseReferences.linearAcceleration = - utils::fromIgnitionVector(baseLinearAccelerationTarget); + utils::fromGzVector(baseLinearAccelerationTarget); baseReferences.angularAcceleration = - utils::fromIgnitionVector(baseAngularAccelerationTarget); + utils::fromGzVector(baseAngularAccelerationTarget); return true; } bool ControllerRunner::Impl::updateJointReferencesfromECM( - ignition::gazebo::EntityComponentManager& /*ecm*/) + gz::sim::EntityComponentManager& /*ecm*/) { assert(controllerInterfaces.joints); @@ -390,7 +390,7 @@ void ControllerRunner::Impl::printControllerContext( std::cout << context->ToString("") << std::endl; } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( scenario::plugins::gazebo::ControllerRunner, scenario::plugins::gazebo::ControllerRunner::System, scenario::plugins::gazebo::ControllerRunner::ISystemConfigure, diff --git a/scenario/src/plugins/ControllerRunner/ControllerRunner.h b/scenario/src/plugins/ControllerRunner/ControllerRunner.h index c0e991634..9e333a507 100644 --- a/scenario/src/plugins/ControllerRunner/ControllerRunner.h +++ b/scenario/src/plugins/ControllerRunner/ControllerRunner.h @@ -27,10 +27,10 @@ #ifndef SCENARIO_PLUGINS_GAZEBO_CONTROLLERRUNNER_H #define SCENARIO_PLUGINS_GAZEBO_CONTROLLERRUNNER_H -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -40,21 +40,21 @@ namespace scenario::plugins::gazebo { } // namespace scenario::plugins::gazebo class scenario::plugins::gazebo::ControllerRunner final - : public ignition::gazebo::System - , public ignition::gazebo::ISystemConfigure - , public ignition::gazebo::ISystemPreUpdate + : public gz::sim::System + , public gz::sim::ISystemConfigure + , public gz::sim::ISystemPreUpdate { public: ControllerRunner(); ~ControllerRunner() override; - void Configure(const ignition::gazebo::Entity& entity, + void Configure(const gz::sim::Entity& entity, const std::shared_ptr& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) override; + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& eventMgr) override; - void PreUpdate(const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& ecm) override; + void PreUpdate(const gz::sim::UpdateInfo& info, + gz::sim::EntityComponentManager& ecm) override; private: class Impl; diff --git a/scenario/src/plugins/JointController/CMakeLists.txt b/scenario/src/plugins/JointController/CMakeLists.txt index da287019a..701cfa55a 100644 --- a/scenario/src/plugins/JointController/CMakeLists.txt +++ b/scenario/src/plugins/JointController/CMakeLists.txt @@ -32,7 +32,7 @@ add_library(JointController SHARED target_link_libraries(JointController PUBLIC - ${ignition-gazebo.core} + ${gz-sim.core} PRIVATE ScenarioGazebo::ScenarioGazebo ScenarioGazebo::ExtraComponents) diff --git a/scenario/src/plugins/JointController/JointController.cpp b/scenario/src/plugins/JointController/JointController.cpp index 604bf3b65..9109d0498 100644 --- a/scenario/src/plugins/JointController/JointController.cpp +++ b/scenario/src/plugins/JointController/JointController.cpp @@ -35,13 +35,13 @@ #include "scenario/gazebo/components/JointVelocityTarget.h" #include "scenario/gazebo/helpers.h" -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -56,13 +56,13 @@ using namespace scenario::plugins::gazebo; class JointController::Impl { public: - ignition::gazebo::Entity modelEntity; + gz::sim::Entity modelEntity; std::shared_ptr model; std::chrono::steady_clock::duration prevUpdateTime{0}; static bool runPIDController(scenario::gazebo::Joint& joint, const bool computeNewForce, - ignition::math::PID& pid, + gz::math::PID& pid, const std::chrono::steady_clock::duration& dt, const std::vector& reference, const std::vector& current); @@ -76,14 +76,14 @@ JointController::JointController() JointController::~JointController() = default; void JointController::Configure( - const ignition::gazebo::Entity& entity, + const gz::sim::Entity& entity, const std::shared_ptr& /*sdf*/, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& eventMgr) { // Check if the model already has a JointController plugin if (ecm.EntityHasComponentType( - entity, ignition::gazebo::components::JointController::typeId)) { + entity, gz::sim::components::JointController::typeId)) { sError << "The model already has a JointController plugin" << std::endl; return; } @@ -107,12 +107,12 @@ void JointController::Configure( } // Add the JointController component to the model - utils::setComponentData( + utils::setComponentData( &ecm, entity, true); } -void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& ecm) +void JointController::PreUpdate(const gz::sim::UpdateInfo& info, + gz::sim::EntityComponentManager& ecm) { if (info.paused) { return; @@ -169,37 +169,37 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, } const auto positionControlledJoints = ecm.EntitiesByComponents( - ignition::gazebo::components::Joint(), - ignition::gazebo::components::ParentEntity(pImpl->modelEntity), - ignition::gazebo::components::JointControlMode( + gz::sim::components::Joint(), + gz::sim::components::ParentEntity(pImpl->modelEntity), + gz::sim::components::JointControlMode( core::JointControlMode::Position)); const auto positionInterpolatedControlledJoints = ecm.EntitiesByComponents( - ignition::gazebo::components::Joint(), - ignition::gazebo::components::ParentEntity(pImpl->modelEntity), - ignition::gazebo::components::JointControlMode( + gz::sim::components::Joint(), + gz::sim::components::ParentEntity(pImpl->modelEntity), + gz::sim::components::JointControlMode( core::JointControlMode::PositionInterpolated)); const auto velocityControlledJoints = ecm.EntitiesByComponents( - ignition::gazebo::components::Joint(), - ignition::gazebo::components::ParentEntity(pImpl->modelEntity), - ignition::gazebo::components::JointControlMode( + gz::sim::components::Joint(), + gz::sim::components::ParentEntity(pImpl->modelEntity), + gz::sim::components::JointControlMode( core::JointControlMode::Velocity)); const auto velocityFollowerDartControlledJoints = ecm.EntitiesByComponents( - ignition::gazebo::components::Joint(), - ignition::gazebo::components::ParentEntity(pImpl->modelEntity), - ignition::gazebo::components::JointControlMode( + gz::sim::components::Joint(), + gz::sim::components::ParentEntity(pImpl->modelEntity), + gz::sim::components::JointControlMode( core::JointControlMode::VelocityFollowerDart)); // Update PIDs for Revolute and Prismatic joints controlled in Position for (const auto jointEntity : positionControlledJoints) { - ignition::math::PID& pid = utils::getExistingComponentData< // - ignition::gazebo::components::JointPID>(&ecm, jointEntity); + gz::math::PID& pid = utils::getExistingComponentData< // + gz::sim::components::JointPID>(&ecm, jointEntity); std::string& jointName = utils::getExistingComponentData< // - ignition::gazebo::components::Name>(&ecm, jointEntity); + gz::sim::components::Name>(&ecm, jointEntity); auto joint = pImpl->model->getJoint(jointName); auto jointGazebo = std::static_pointer_cast(joint); @@ -208,7 +208,7 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, const std::vector& positionTarget = utils::getExistingComponentData< - ignition::gazebo::components::JointPositionTarget>(&ecm, + gz::sim::components::JointPositionTarget>(&ecm, jointEntity); if (!Impl::runPIDController(*jointGazebo, @@ -228,11 +228,11 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, // Update PIDs for Revolute and Prismatic joints controlled in Velocity for (const auto jointEntity : velocityControlledJoints) { - ignition::math::PID& pid = utils::getExistingComponentData< - ignition::gazebo::components::JointPID>(&ecm, jointEntity); + gz::math::PID& pid = utils::getExistingComponentData< + gz::sim::components::JointPID>(&ecm, jointEntity); std::string& jointName = - utils::getExistingComponentData( + utils::getExistingComponentData( &ecm, jointEntity); auto joint = pImpl->model->getJoint(jointName); @@ -242,7 +242,7 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, const std::vector& velocityTarget = utils::getExistingComponentData< - ignition::gazebo::components::JointVelocityTarget>(&ecm, + gz::sim::components::JointVelocityTarget>(&ecm, jointEntity); if (!Impl::runPIDController(*jointGazebo, @@ -263,18 +263,18 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, for (const auto jointEntity : velocityFollowerDartControlledJoints) { const std::string& jointName = - utils::getExistingComponentData( + utils::getExistingComponentData( &ecm, jointEntity); const auto joint = pImpl->model->getJoint(jointName); const std::vector& velocityTarget = utils::getExistingComponentData< - ignition::gazebo::components::JointVelocityTarget>(&ecm, + gz::sim::components::JointVelocityTarget>(&ecm, jointEntity); auto& jointVelocityCmd = utils::getComponentData< // - ignition::gazebo::components::JointVelocityCmd>(&ecm, jointEntity); + gz::sim::components::JointVelocityCmd>(&ecm, jointEntity); if (jointVelocityCmd.size() != joint->dofs()) { assert(jointVelocityCmd.size() == 0); @@ -289,7 +289,7 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, bool JointController::Impl::runPIDController( scenario::gazebo::Joint& joint, const bool computeNewForce, - ignition::math::PID& pid, + gz::math::PID& pid, const std::chrono::steady_clock::duration& dt, const std::vector& reference, const std::vector& current) @@ -330,7 +330,7 @@ bool JointController::Impl::runPIDController( return false; }; -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( scenario::plugins::gazebo::JointController, scenario::plugins::gazebo::JointController::System, scenario::plugins::gazebo::JointController::ISystemConfigure, diff --git a/scenario/src/plugins/JointController/JointController.h b/scenario/src/plugins/JointController/JointController.h index 932dbb738..14ef5dda5 100644 --- a/scenario/src/plugins/JointController/JointController.h +++ b/scenario/src/plugins/JointController/JointController.h @@ -27,10 +27,10 @@ #ifndef SCENARIO_PLUGINS_GAZEBO_JOINTCONTROLLER_H #define SCENARIO_PLUGINS_GAZEBO_JOINTCONTROLLER_H -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -40,9 +40,9 @@ namespace scenario::plugins::gazebo { } // namespace scenario::plugins::gazebo class scenario::plugins::gazebo::JointController final - : public ignition::gazebo::System - , public ignition::gazebo::ISystemConfigure - , public ignition::gazebo::ISystemPreUpdate + : public gz::sim::System + , public gz::sim::ISystemConfigure + , public gz::sim::ISystemPreUpdate { private: class Impl; @@ -52,13 +52,13 @@ class scenario::plugins::gazebo::JointController final JointController(); ~JointController() override; - void Configure(const ignition::gazebo::Entity& entity, + void Configure(const gz::sim::Entity& entity, const std::shared_ptr& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) override; + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& eventMgr) override; - void PreUpdate(const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& ecm) override; + void PreUpdate(const gz::sim::UpdateInfo& info, + gz::sim::EntityComponentManager& ecm) override; }; #endif // SCENARIO_PLUGINS_GAZEBO_JOINTCONTROLLER_H diff --git a/scenario/src/plugins/Physics/CMakeLists.txt b/scenario/src/plugins/Physics/CMakeLists.txt index e7dfe0c03..fe620257b 100644 --- a/scenario/src/plugins/Physics/CMakeLists.txt +++ b/scenario/src/plugins/Physics/CMakeLists.txt @@ -34,8 +34,8 @@ add_library(PhysicsSystem SHARED target_link_libraries(PhysicsSystem PUBLIC - ${ignition-gazebo.core} - ${ignition-physics.ignition-physics} + ${gz-sim.core} + ${gz-physics.gz-physics} PRIVATE ScenarioGazebo::ScenarioGazebo ScenarioGazebo::ExtraComponents) @@ -44,7 +44,7 @@ target_include_directories(PhysicsSystem PRIVATE $) if(ENABLE_PROFILER) - target_compile_definitions(PhysicsSystem PRIVATE "IGN_PROFILER_ENABLE=1") + target_compile_definitions(PhysicsSystem PRIVATE "GZ_PROFILER_ENABLE=1") endif() # =================== diff --git a/scenario/src/plugins/Physics/CanonicalLinkModelTracker.hh b/scenario/src/plugins/Physics/CanonicalLinkModelTracker.hh index 4f6f01443..c499023c8 100644 --- a/scenario/src/plugins/Physics/CanonicalLinkModelTracker.hh +++ b/scenario/src/plugins/Physics/CanonicalLinkModelTracker.hh @@ -14,21 +14,21 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_PHYSICS_CANONICAL_LINK_MODEL_TRACKER_HH_ -#define IGNITION_GAZEBO_SYSTEMS_PHYSICS_CANONICAL_LINK_MODEL_TRACKER_HH_ +#ifndef GZ_SIM_SYSTEMS_PHYSICS_CANONICAL_LINK_MODEL_TRACKER_HH_ +#define GZ_SIM_SYSTEMS_PHYSICS_CANONICAL_LINK_MODEL_TRACKER_HH_ #include #include -#include "ignition/gazebo/Entity.hh" -#include "ignition/gazebo/EntityComponentManager.hh" -#include "ignition/gazebo/components/CanonicalLink.hh" -#include "ignition/gazebo/components/Model.hh" -#include "ignition/gazebo/config.hh" +#include "gz/sim/Entity.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/components/CanonicalLink.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/config.hh" -namespace ignition::gazebo +namespace gz::sim { -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems::physics_system { /// \brief Helper class that keeps track of which models have a particular diff --git a/scenario/src/plugins/Physics/EntityFeatureMap.hh b/scenario/src/plugins/Physics/EntityFeatureMap.hh index 933accc9f..4bbfe4941 100644 --- a/scenario/src/plugins/Physics/EntityFeatureMap.hh +++ b/scenario/src/plugins/Physics/EntityFeatureMap.hh @@ -15,23 +15,23 @@ * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_ -#define IGNITION_GAZEBO_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_ +#ifndef GZ_SIM_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_ +#define GZ_SIM_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include -#include "ignition/gazebo/Entity.hh" +#include "gz/sim/Entity.hh" -namespace ignition::gazebo +namespace gz::sim { -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems::physics_system { // \brief Helper class that associates Gazebo entities with Physics entities @@ -44,7 +44,7 @@ namespace systems::physics_system // and the memory associated with the physics entities can be freed. // // DEV WARNING: There is an implicit conversion between physics EntityPtr and - // std::size_t in ign-physics. This seems also implicitly convert between + // std::size_t in gz-physics. This seems also implicitly convert between // EntityPtr and gazebo Entity. Therefore, any member function that takes a // gazebo Entity can accidentally take an EntityPtr. To prevent this, for // every function that takes a gazebo Entity, we should always have an @@ -94,7 +94,7 @@ namespace systems::physics_system /// requested feature. public: template PhysicsEntityPtr - EntityCast(gazebo::Entity _entity) const + EntityCast(gz::sim::Entity _entity) const { // Using constexpr to limit compiler error message to the static_assert // cppcheck-suppress syntaxError diff --git a/scenario/src/plugins/Physics/Physics.cc b/scenario/src/plugins/Physics/Physics.cc index e800d2cd1..ad76baae2 100644 --- a/scenario/src/plugins/Physics/Physics.cc +++ b/scenario/src/plugins/Physics/Physics.cc @@ -18,10 +18,10 @@ // clang-format off #include "Physics.hh" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -32,45 +32,45 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // SDF #include @@ -82,53 +82,53 @@ #include #include -#include "ignition/gazebo/EntityComponentManager.hh" -#include "ignition/gazebo/Model.hh" -#include "ignition/gazebo/Util.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" // Components -#include "ignition/gazebo/components/AngularAcceleration.hh" -#include "ignition/gazebo/components/AngularVelocity.hh" -#include "ignition/gazebo/components/AngularVelocityCmd.hh" -#include "ignition/gazebo/components/AxisAlignedBox.hh" -#include "ignition/gazebo/components/BatterySoC.hh" -#include "ignition/gazebo/components/CanonicalLink.hh" -#include "ignition/gazebo/components/ChildLinkName.hh" -#include "ignition/gazebo/components/Collision.hh" -#include "ignition/gazebo/components/ContactSensorData.hh" -#include "ignition/gazebo/components/Geometry.hh" -#include "ignition/gazebo/components/Gravity.hh" -#include "ignition/gazebo/components/Inertial.hh" -#include "ignition/gazebo/components/DetachableJoint.hh" -#include "ignition/gazebo/components/Joint.hh" -#include "ignition/gazebo/components/JointAxis.hh" -#include "ignition/gazebo/components/JointPosition.hh" -#include "ignition/gazebo/components/JointPositionReset.hh" -#include "ignition/gazebo/components/JointType.hh" -#include "ignition/gazebo/components/JointVelocity.hh" -#include "ignition/gazebo/components/JointVelocityCmd.hh" -#include "ignition/gazebo/components/JointVelocityReset.hh" -#include "ignition/gazebo/components/LinearAcceleration.hh" -#include "ignition/gazebo/components/LinearVelocity.hh" -#include "ignition/gazebo/components/LinearVelocityCmd.hh" -#include "ignition/gazebo/components/Link.hh" -#include "ignition/gazebo/components/Model.hh" -#include "ignition/gazebo/components/Name.hh" -#include "ignition/gazebo/components/ParentEntity.hh" -#include "ignition/gazebo/components/ParentLinkName.hh" -#include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh" -#include "ignition/gazebo/components/JointTransmittedWrench.hh" -#include "ignition/gazebo/components/JointForceCmd.hh" -#include "ignition/gazebo/components/Physics.hh" -#include "ignition/gazebo/components/PhysicsEnginePlugin.hh" -#include "ignition/gazebo/components/Pose.hh" -#include "ignition/gazebo/components/PoseCmd.hh" -#include "ignition/gazebo/components/SelfCollide.hh" -#include "ignition/gazebo/components/SlipComplianceCmd.hh" -#include "ignition/gazebo/components/Static.hh" -#include "ignition/gazebo/components/ThreadPitch.hh" -#include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/components/HaltMotion.hh" +#include "gz/sim/components/AngularAcceleration.hh" +#include "gz/sim/components/AngularVelocity.hh" +#include "gz/sim/components/AngularVelocityCmd.hh" +#include "gz/sim/components/AxisAlignedBox.hh" +#include "gz/sim/components/BatterySoC.hh" +#include "gz/sim/components/CanonicalLink.hh" +#include "gz/sim/components/ChildLinkName.hh" +#include "gz/sim/components/Collision.hh" +#include "gz/sim/components/ContactSensorData.hh" +#include "gz/sim/components/Geometry.hh" +#include "gz/sim/components/Gravity.hh" +#include "gz/sim/components/Inertial.hh" +#include "gz/sim/components/DetachableJoint.hh" +#include "gz/sim/components/Joint.hh" +#include "gz/sim/components/JointAxis.hh" +#include "gz/sim/components/JointPosition.hh" +#include "gz/sim/components/JointPositionReset.hh" +#include "gz/sim/components/JointType.hh" +#include "gz/sim/components/JointVelocity.hh" +#include "gz/sim/components/JointVelocityCmd.hh" +#include "gz/sim/components/JointVelocityReset.hh" +#include "gz/sim/components/LinearAcceleration.hh" +#include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/components/LinearVelocityCmd.hh" +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/ParentLinkName.hh" +#include "gz/sim/components/ExternalWorldWrenchCmd.hh" +#include "gz/sim/components/JointTransmittedWrench.hh" +#include "gz/sim/components/JointForceCmd.hh" +#include "gz/sim/components/Physics.hh" +#include "gz/sim/components/PhysicsEnginePlugin.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/PoseCmd.hh" +#include "gz/sim/components/SelfCollide.hh" +#include "gz/sim/components/SlipComplianceCmd.hh" +#include "gz/sim/components/Static.hh" +#include "gz/sim/components/ThreadPitch.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/components/HaltMotion.hh" #include "CanonicalLinkModelTracker.hh" #include "EntityFeatureMap.hh" @@ -137,54 +137,54 @@ #include "scenario/gazebo/components/ExternalWorldWrenchCmdWithDuration.h" #include "scenario/gazebo/components/HistoryOfAppliedJointForces.h" #include "scenario/gazebo/components/JointAcceleration.h" -#include +#include #include "scenario/gazebo/components/SimulatedTime.h" -using namespace ignition; -using namespace ignition::gazebo; -using namespace ignition::gazebo::systems; -using namespace ignition::gazebo::systems::physics_system; -namespace components = ignition::gazebo::components; +using namespace gz; +using namespace gz::sim; +using namespace gz::sim::systems; +using namespace gz::sim::systems::physics_system; +namespace components = gz::sim::components; // Private data class. -class ignition::gazebo::systems::PhysicsPrivate +class gz::sim::systems::PhysicsPrivate { /// \brief This is the minimum set of features that any physics engine must /// implement to be supported by this system. /// New features can't be added to this list in minor / patch releases, in /// order to maintain backwards compatibility with downstream physics plugins. - public: struct MinimumFeatureList : ignition::physics::FeatureList< - ignition::physics::FindFreeGroupFeature, - ignition::physics::SetFreeGroupWorldPose, - ignition::physics::FreeGroupFrameSemantics, - ignition::physics::LinkFrameSemantics, - ignition::physics::ForwardStep, - ignition::physics::RemoveModelFromWorld, - ignition::physics::sdf::ConstructSdfLink, - ignition::physics::sdf::ConstructSdfModel, - ignition::physics::sdf::ConstructSdfWorld + public: struct MinimumFeatureList : gz::physics::FeatureList< + gz::physics::FindFreeGroupFeature, + gz::physics::SetFreeGroupWorldPose, + gz::physics::FreeGroupFrameSemantics, + gz::physics::LinkFrameSemantics, + gz::physics::ForwardStep, + gz::physics::RemoveModelFromWorld, + gz::physics::sdf::ConstructSdfLink, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld >{}; /// \brief Engine type with just the minimum features. - public: using EnginePtrType = ignition::physics::EnginePtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using EnginePtrType = gz::physics::EnginePtr< + gz::physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief World type with just the minimum features. - public: using WorldPtrType = ignition::physics::WorldPtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using WorldPtrType = gz::physics::WorldPtr< + gz::physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Model type with just the minimum features. - public: using ModelPtrType = ignition::physics::ModelPtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using ModelPtrType = gz::physics::ModelPtr< + gz::physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Link type with just the minimum features. - public: using LinkPtrType = ignition::physics::LinkPtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using LinkPtrType = gz::physics::LinkPtr< + gz::physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Free group type with just the minimum features. - public: using FreeGroupPtrType = ignition::physics::FreeGroupPtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using FreeGroupPtrType = gz::physics::FreeGroupPtr< + gz::physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Create physics entities /// \param[in] _ecm Constant reference to ECM. @@ -221,13 +221,13 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Update physics from components /// \param[in] _ecm Mutable reference to ECM. public: void UpdatePhysics(EntityComponentManager &_ecm, - const ignition::gazebo::UpdateInfo& _info); + const gz::sim::UpdateInfo& _info); /// \brief Step the simulation for each world /// \param[in] _dt Duration /// \returns Output data from the physics engine (this currently contains /// data for links that experienced a pose change in the physics step) - public: ignition::physics::ForwardStep::Output Step( + public: gz::physics::ForwardStep::Output Step( const std::chrono::steady_clock::duration &_dt); /// \brief Get data of links that were updated in the latest physics step. @@ -242,7 +242,7 @@ class ignition::gazebo::systems::PhysicsPrivate /// properly (models must be updated in topological order). public: std::map ChangedLinks( EntityComponentManager &_ecm, - const ignition::physics::ForwardStep::Output &_updatedLinks); + const gz::physics::ForwardStep::Output &_updatedLinks); /// \brief Helper function to update the pose of a model. /// \param[in] _model The model to update. @@ -274,14 +274,14 @@ class ignition::gazebo::systems::PhysicsPrivate /// value is the updated frame data corresponding to that entity. public: void UpdateSim(EntityComponentManager &_ecm, std::map &_linkFrameData, - const ignition::gazebo::UpdateInfo &_info); + const gz::sim::UpdateInfo &_info); /// \brief Update collision components from physics simulation /// \param[in] _ecm Mutable reference to ECM. public: void UpdateCollisions(EntityComponentManager &_ecm); /// \brief FrameData relative to world at a given offset pose - /// \param[in] _link ign-physics link + /// \param[in] _link gz-physics link /// \param[in] _pose Offset pose in which to compute the frame data /// \returns FrameData at the given offset pose public: physics::FrameData3d LinkFrameDataAtOffset( @@ -292,7 +292,7 @@ class ignition::gazebo::systems::PhysicsPrivate /// \param[in] _from An ancestor of the _to entity. /// \param[in] _to A descendant of the _from entity. /// \return Pose transform between the two entities - public: ignition::math::Pose3d RelativePose(const Entity &_from, + public: gz::math::Pose3d RelativePose(const Entity &_from, const Entity &_to, const EntityComponentManager &_ecm) const; /// \brief Cache the top-level model for each entity. @@ -305,7 +305,7 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Keep track of poses for links attached to non-static models. /// This allows for skipping pose updates if a link's pose didn't change /// after a physics step. - public: std::unordered_map linkWorldPoses; + public: std::unordered_map linkWorldPoses; /// \brief Keep a mapping of canonical links to models that have this /// canonical link. Useful for updating model poses efficiently after a @@ -328,7 +328,7 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief used to store whether physics objects have been created. public: bool initialized = false; - /// \brief Pointer to the underlying ign-physics Engine entity. + /// \brief Pointer to the underlying gz-physics Engine entity. public: EnginePtrType engine = nullptr; /// \brief Vector3d equality comparison function. @@ -405,7 +405,7 @@ class ignition::gazebo::systems::PhysicsPrivate }}; /// \brief Environment variable which holds paths to look for engine plugins - public: std::string pluginPathEnv = "IGN_GAZEBO_PHYSICS_ENGINE_PATH"; + public: std::string pluginPathEnv = "GZ_SIM_PHYSICS_ENGINE_PATH"; ////////////////////////////////////////////////// ////////////// Optional Features ///////////////// @@ -418,18 +418,18 @@ class ignition::gazebo::systems::PhysicsPrivate public: struct FrictionPyramidSlipComplianceFeatureList : physics::FeatureList< MinimumFeatureList, - ignition::physics::GetShapeFrictionPyramidSlipCompliance, - ignition::physics::SetShapeFrictionPyramidSlipCompliance>{}; + gz::physics::GetShapeFrictionPyramidSlipCompliance, + gz::physics::SetShapeFrictionPyramidSlipCompliance>{}; ////////////////////////////////////////////////// // Joints /// \brief Feature list to handle joints. - public: struct JointFeatureList : ignition::physics::FeatureList< + public: struct JointFeatureList : gz::physics::FeatureList< MinimumFeatureList, - ignition::physics::GetBasicJointProperties, - ignition::physics::GetBasicJointState, - ignition::physics::SetBasicJointState, - ignition::physics::sdf::ConstructSdfJoint>{}; + gz::physics::GetBasicJointProperties, + gz::physics::GetBasicJointState, + gz::physics::SetBasicJointState, + gz::physics::sdf::ConstructSdfJoint>{}; ////////////////////////////////////////////////// @@ -452,40 +452,40 @@ class ignition::gazebo::systems::PhysicsPrivate // Collisions /// \brief Feature list to handle collisions. - public: struct CollisionFeatureList : ignition::physics::FeatureList< + public: struct CollisionFeatureList : gz::physics::FeatureList< MinimumFeatureList, - ignition::physics::GetContactsFromLastStepFeature, - ignition::physics::sdf::ConstructSdfCollision>{}; + gz::physics::GetContactsFromLastStepFeature, + gz::physics::sdf::ConstructSdfCollision>{}; /// \brief Collision type with collision features. - public: using ShapePtrType = ignition::physics::ShapePtr< - ignition::physics::FeaturePolicy3d, CollisionFeatureList>; + public: using ShapePtrType = gz::physics::ShapePtr< + gz::physics::FeaturePolicy3d, CollisionFeatureList>; /// \brief World type with just the minimum features. Non-pointer. - public: using WorldShapeType = ignition::physics::World< - ignition::physics::FeaturePolicy3d, CollisionFeatureList>; + public: using WorldShapeType = gz::physics::World< + gz::physics::FeaturePolicy3d, CollisionFeatureList>; ////////////////////////////////////////////////// // Collision filtering with bitmasks /// \brief Feature list to filter collisions with bitmasks. - public: struct CollisionMaskFeatureList : ignition::physics::FeatureList< + public: struct CollisionMaskFeatureList : gz::physics::FeatureList< CollisionFeatureList, - ignition::physics::CollisionFilterMaskFeature>{}; + gz::physics::CollisionFilterMaskFeature>{}; ////////////////////////////////////////////////// // Link force /// \brief Feature list for applying forces to links. - public: struct LinkForceFeatureList : ignition::physics::FeatureList< - ignition::physics::AddLinkExternalForceTorque>{}; + public: struct LinkForceFeatureList : gz::physics::FeatureList< + gz::physics::AddLinkExternalForceTorque>{}; ////////////////////////////////////////////////// // Bounding box /// \brief Feature list for model bounding box. - public: struct BoundingBoxFeatureList : ignition::physics::FeatureList< + public: struct BoundingBoxFeatureList : gz::physics::FeatureList< MinimumFeatureList, - ignition::physics::GetModelBoundingBox>{}; + gz::physics::GetModelBoundingBox>{}; ////////////////////////////////////////////////// @@ -497,8 +497,8 @@ class ignition::gazebo::systems::PhysicsPrivate ////////////////////////////////////////////////// // World velocity command public: struct WorldVelocityCommandFeatureList : - ignition::physics::FeatureList< - ignition::physics::SetFreeGroupWorldVelocity>{}; + gz::physics::FeatureList< + gz::physics::SetFreeGroupWorldVelocity>{}; ////////////////////////////////////////////////// @@ -517,29 +517,29 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Feature list for heightmaps. /// Include MinimumFeatureList so created collision can be automatically /// up-cast. - public: struct HeightmapFeatureList : ignition::physics::FeatureList< + public: struct HeightmapFeatureList : gz::physics::FeatureList< CollisionFeatureList, physics::heightmap::AttachHeightmapShapeFeature>{}; ////////////////////////////////////////////////// // Collision detector /// \brief Feature list for setting and getting the collision detector - public: struct CollisionDetectorFeatureList : ignition::physics::FeatureList< - ignition::physics::CollisionDetector>{}; + public: struct CollisionDetectorFeatureList : gz::physics::FeatureList< + gz::physics::CollisionDetector>{}; ////////////////////////////////////////////////// // Solver /// \brief Feature list for setting and getting the solver - public: struct SolverFeatureList : ignition::physics::FeatureList< - ignition::physics::Solver>{}; + public: struct SolverFeatureList : gz::physics::FeatureList< + gz::physics::Solver>{}; ////////////////////////////////////////////////// // Nested Models /// \brief Feature list to construct nested models - public: struct NestedModelFeatureList : ignition::physics::FeatureList< + public: struct NestedModelFeatureList : gz::physics::FeatureList< MinimumFeatureList, - ignition::physics::sdf::ConstructSdfNestedModel>{}; + gz::physics::sdf::ConstructSdfNestedModel>{}; ////////////////////////////////////////////////// /// \brief World EntityFeatureMap @@ -552,7 +552,7 @@ class ignition::gazebo::systems::PhysicsPrivate SolverFeatureList>; /// \brief A map between world entity ids in the ECM to World Entities in - /// ign-physics. + /// gz-physics. public: WorldEntityMap entityWorldMap; /// \brief Model EntityFeatureMap @@ -564,7 +564,7 @@ class ignition::gazebo::systems::PhysicsPrivate NestedModelFeatureList>; /// \brief A map between model entity ids in the ECM to Model Entities in - /// ign-physics. + /// gz-physics. public: ModelEntityMap entityModelMap; /// \brief Link EntityFeatureMap @@ -578,7 +578,7 @@ class ignition::gazebo::systems::PhysicsPrivate MeshFeatureList>; /// \brief A map between link entity ids in the ECM to Link Entities in - /// ign-physics. + /// gz-physics. public: EntityLinkMap entityLinkMap; /// \brief Joint EntityFeatureMap @@ -591,7 +591,7 @@ class ignition::gazebo::systems::PhysicsPrivate >; /// \brief A map between joint entity ids in the ECM to Joint Entities in - /// ign-physics + /// gz-physics public: EntityJointMap entityJointMap; /// \brief Collision EntityFeatureMap @@ -603,7 +603,7 @@ class ignition::gazebo::systems::PhysicsPrivate >; /// \brief A map between collision entity ids in the ECM to Shape Entities in - /// ign-physics. + /// gz-physics. public: EntityCollisionMap entityCollisionMap; /// \brief FreeGroup EntityFeatureMap @@ -614,7 +614,7 @@ class ignition::gazebo::systems::PhysicsPrivate >; /// \brief A map between collision entity ids in the ECM to FreeGroup Entities - /// in ign-physics. + /// in gz-physics. public: EntityFreeGroupMap entityFreeGroupMap; /// \brief Boolean value that is true only the first call of Configure and @@ -652,7 +652,7 @@ void Physics::Configure(const Entity &_entity, // 3. Use DART by default if (pluginLib.empty()) { - pluginLib = "libignition-physics-dartsim-plugin.so"; + pluginLib = "libgz-physics-dartsim-plugin.so"; } // Update component @@ -669,26 +669,26 @@ void Physics::Configure(const Entity &_entity, // Find engine shared library // Look in: // * Paths from environment variable - // * Engines installed with ign-physics + // * Engines installed with gz-physics common::SystemPaths systemPaths; systemPaths.SetPluginPathEnv(this->dataPtr->pluginPathEnv); - systemPaths.AddPluginPaths({IGNITION_PHYSICS_ENGINE_INSTALL_DIR}); + systemPaths.AddPluginPaths({GZ_PHYSICS_ENGINE_INSTALL_DIR}); auto pathToLib = systemPaths.FindSharedLibrary(pluginLib); if (pathToLib.empty()) { - ignerr << "Failed to find plugin [" << pluginLib + gzerr << "Failed to find plugin [" << pluginLib << "]. Have you checked the " << this->dataPtr->pluginPathEnv << " environment variable?" << std::endl; return; } // Load engine plugin - ignition::plugin::Loader pluginLoader; + gz::plugin::Loader pluginLoader; auto plugins = pluginLoader.LoadLib(pathToLib); if (plugins.empty()) { - ignerr << "Unable to load the [" << pathToLib << "] library.\n"; + gzerr << "Unable to load the [" << pathToLib << "] library.\n"; return; } @@ -697,7 +697,7 @@ void Physics::Configure(const Entity &_entity, physics::FeaturePolicy3d>>(); if (classNames.empty()) { - ignerr << "No physics plugins found in library [" << pathToLib << "]." + gzerr << "No physics plugins found in library [" << pathToLib << "]." << std::endl; return; } @@ -709,24 +709,24 @@ void Physics::Configure(const Entity &_entity, if (!plugin) { - ignwarn << "Failed to instantiate [" << className << "] from [" + gzwarn << "Failed to instantiate [" << className << "] from [" << pathToLib << "]" << std::endl; continue; } - this->dataPtr->engine = ignition::physics::RequestEngine< - ignition::physics::FeaturePolicy3d, + this->dataPtr->engine = gz::physics::RequestEngine< + gz::physics::FeaturePolicy3d, PhysicsPrivate::MinimumFeatureList>::From(plugin); if (nullptr != this->dataPtr->engine) { - igndbg << "Loaded [" << className << "] from library [" + gzdbg << "Loaded [" << className << "] from library [" << pathToLib << "]" << std::endl; break; } - auto missingFeatures = ignition::physics::RequestEngine< - ignition::physics::FeaturePolicy3d, + auto missingFeatures = gz::physics::RequestEngine< + gz::physics::FeaturePolicy3d, PhysicsPrivate::MinimumFeatureList>::MissingFeatureNames(plugin); std::stringstream msg; @@ -736,12 +736,12 @@ void Physics::Configure(const Entity &_entity, { msg << "- " << feature << std::endl; } - ignwarn << msg.str(); + gzwarn << msg.str(); } if (nullptr == this->dataPtr->engine) { - ignerr << "Failed to load a valid physics engine from [" << pathToLib + gzerr << "Failed to load a valid physics engine from [" << pathToLib << "]." << std::endl; } @@ -753,12 +753,12 @@ Physics::~Physics() = default; ////////////////////////////////////////////////// void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { - IGN_PROFILE("Physics::Update"); + GZ_PROFILE("Physics::Update"); // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { - ignwarn << "Detected jump back in time [" + gzwarn << "Detected jump back in time [" << std::chrono::duration_cast(_info.dt).count() << "s]. System may not work properly." << std::endl; } @@ -779,7 +779,7 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { this->dataPtr->CreatePhysicsEntities(_ecm); this->dataPtr->UpdatePhysics(_ecm, _info); - ignition::physics::ForwardStep::Output stepOutput; + gz::physics::ForwardStep::Output stepOutput; // Only step if not paused. if (!_info.paused) { @@ -827,7 +827,7 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) // Check if world already exists if (this->entityWorldMap.HasEntity(_entity)) { - ignwarn << "World entity [" << _entity + gzwarn << "World entity [" << _entity << "] marked as new, but it's already on the map." << std::endl; return true; @@ -852,7 +852,7 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to set physics options, but the " + gzdbg << "Attempting to set physics options, but the " << "phyiscs engine doesn't support feature " << "[CollisionDetectorFeature]. Options will be ignored." << std::endl; @@ -878,7 +878,7 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to set physics options, but the " + gzdbg << "Attempting to set physics options, but the " << "phyiscs engine doesn't support feature " << "[SolverFeature]. Options will be ignored." << std::endl; @@ -919,7 +919,7 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) // Check if model already exists if (this->entityModelMap.HasEntity(_entity)) { - ignwarn << "Model entity [" << _entity + gzwarn << "Model entity [" << _entity << "] marked as new, but it's already on the map." << std::endl; return true; @@ -957,7 +957,7 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to construct nested models, but the " + gzdbg << "Attempting to construct nested models, but the " << "phyiscs engine doesn't support feature " << "[ConstructSdfNestedModelFeature]. " << "Nested model will be ignored." @@ -992,7 +992,7 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to construct nested models, but the " + gzdbg << "Attempting to construct nested models, but the " << "physics engine doesn't support feature " << "[ConstructSdfNestedModelFeature]. " << "Nested model will be ignored." @@ -1020,14 +1020,14 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) } else { - ignerr << "Model: '" << _name->Data() << "' not loaded. " + gzerr << "Model: '" << _name->Data() << "' not loaded. " << "Failed to create nested model." << std::endl; } } else { - ignwarn << "Model's parent entity [" << _parent->Data() + gzwarn << "Model's parent entity [" << _parent->Data() << "] not found on world / model map." << std::endl; return true; } @@ -1065,7 +1065,7 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) // Check if link already exists if (this->entityLinkMap.HasEntity(_entity)) { - ignwarn << "Link entity [" << _entity + gzwarn << "Link entity [" << _entity << "] marked as new, but it's already on the map." << std::endl; return true; @@ -1076,7 +1076,7 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) // Check if parent model exists if (!this->entityModelMap.HasEntity(_parent->Data())) { - ignwarn << "Link's parent entity [" << _parent->Data() + gzwarn << "Link's parent entity [" << _parent->Data() << "] not found on model map." << std::endl; return true; } @@ -1138,7 +1138,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) { if (this->entityCollisionMap.HasEntity(_entity)) { - ignwarn << "Collision entity [" << _entity + gzwarn << "Collision entity [" << _entity << "] marked as new, but it's already on the map." << std::endl; return true; @@ -1147,7 +1147,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) // Check if parent link exists if (!this->entityLinkMap.HasEntity(_parent->Data())) { - ignwarn << "Collision's parent entity [" << _parent->Data() + gzwarn << "Collision's parent entity [" << _parent->Data() << "] not found on link map." << std::endl; return true; } @@ -1167,17 +1167,17 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) const sdf::Mesh *meshSdf = _geom->Data().MeshShape(); if (nullptr == meshSdf) { - ignwarn << "Mesh geometry for collision [" << _name->Data() + gzwarn << "Mesh geometry for collision [" << _name->Data() << "] missing mesh shape." << std::endl; return true; } - auto &meshManager = *ignition::common::MeshManager::Instance(); + auto &meshManager = *gz::common::MeshManager::Instance(); auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath()); auto *mesh = meshManager.Load(fullPath); if (nullptr == mesh) { - ignwarn << "Failed to load mesh from [" << fullPath + gzwarn << "Failed to load mesh from [" << fullPath << "]." << std::endl; return true; } @@ -1189,7 +1189,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to process mesh geometries, but the physics" + gzdbg << "Attempting to process mesh geometries, but the physics" << " engine doesn't support feature " << "[AttachMeshShapeFeature]. Meshes will be ignored." << std::endl; @@ -1213,7 +1213,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to process heightmap geometries, but the " + gzdbg << "Attempting to process heightmap geometries, but the " << "physics engine doesn't support feature " << "[AttachHeightmapShapeFeature]. Heightmaps will be " << "ignored." << std::endl; @@ -1225,7 +1225,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) auto heightmapSdf = _geom->Data().HeightmapShape(); if (nullptr == heightmapSdf) { - ignwarn << "Heightmap geometry for collision [" << _name->Data() + gzwarn << "Heightmap geometry for collision [" << _name->Data() << "] missing heightmap shape." << std::endl; return true; } @@ -1234,14 +1234,14 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) heightmapSdf->FilePath()); if (fullPath.empty()) { - ignerr << "Heightmap geometry missing URI" << std::endl; + gzerr << "Heightmap geometry missing URI" << std::endl; return true; } common::ImageHeightmap data; if (data.Load(fullPath) < 0) { - ignerr << "Failed to load heightmap image data from [" << fullPath + gzerr << "Failed to load heightmap image data from [" << fullPath << "]" << std::endl; return true; } @@ -1263,7 +1263,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to process collisions, but the physics " + gzdbg << "Attempting to process collisions, but the physics " << "engine doesn't support feature " << "[ConstructSdfCollision]. Collisions will be ignored." << std::endl; @@ -1278,7 +1278,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) if (nullptr == collisionPtrPhys) { - igndbg << "Failed to create collision [" << _name->Data() + gzdbg << "Failed to create collision [" << _name->Data() << "]. Does the physics engine support geometries of type [" << static_cast(_geom->Data().Type()) << "]?" << std::endl; return true; @@ -1300,7 +1300,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to set collision bitmasks, but the physics " + gzdbg << "Attempting to set collision bitmasks, but the physics " << "engine doesn't support feature [CollisionFilterMask]. " << "Collision bitmasks will be ignored." << std::endl; informed = true; @@ -1349,7 +1349,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) // Check if joint already exists if (this->entityJointMap.HasEntity(_entity)) { - ignwarn << "Joint entity [" << _entity + gzwarn << "Joint entity [" << _entity << "] marked as new, but it's already on the map." << std::endl; return true; @@ -1358,7 +1358,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) // Check if parent model exists if (!this->entityModelMap.HasEntity(_parentModel->Data())) { - ignwarn << "Joint's parent entity [" << _parentModel->Data() + gzwarn << "Joint's parent entity [" << _parentModel->Data() << "] not found on model map." << std::endl; return true; } @@ -1372,7 +1372,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to process joints, but the physics " + gzdbg << "Attempting to process joints, but the physics " << "engine doesn't support joint features. " << "Joints will be ignored." << std::endl; informed = true; @@ -1423,14 +1423,14 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) { if (_jointInfo->Data().jointType != "fixed") { - ignerr << "Detachable joint type [" << _jointInfo->Data().jointType + gzerr << "Detachable joint type [" << _jointInfo->Data().jointType << "] is currently not supported" << std::endl; return true; } // Check if joint already exists if (this->entityJointMap.HasEntity(_entity)) { - ignwarn << "Joint entity [" << _entity + gzwarn << "Joint entity [" << _entity << "] marked as new, but it's already on the map." << std::endl; return true; @@ -1441,7 +1441,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) this->entityLinkMap.Get(_jointInfo->Data().parentLink); if (!parentLinkPhys) { - ignwarn << "DetachableJoint's parent link entity [" + gzwarn << "DetachableJoint's parent link entity [" << _jointInfo->Data().parentLink << "] not found in link map." << std::endl; return true; @@ -1453,7 +1453,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) auto childLinkPhys = this->entityLinkMap.Get(childLinkEntity); if (!childLinkPhys) { - ignwarn << "Failed to find joint's child link [" << childLinkEntity + gzwarn << "Failed to find joint's child link [" << childLinkEntity << "]." << std::endl; return true; } @@ -1466,7 +1466,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to create a detachable joint, but the physics" + gzdbg << "Attempting to create a detachable joint, but the physics" << " engine doesn't support feature " << "[AttachFixedJointFeature]. Detachable joints will be " << "ignored." << std::endl; @@ -1491,7 +1491,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) // We let the joint be at the origin of the child link. jointPtrPhys->SetTransformFromParent(poseParentChild); - igndbg << "Creating detachable joint [" << _entity << "]" + gzdbg << "Creating detachable joint [" << _entity << "]" << std::endl; this->entityJointMap.AddEntity(_entity, jointPtrPhys); this->topLevelModelMap.insert(std::make_pair(_entity, @@ -1499,7 +1499,7 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) } else { - ignwarn << "DetachableJoint could not be created." << std::endl; + gzwarn << "DetachableJoint could not be created." << std::endl; } return true; }; @@ -1556,7 +1556,7 @@ void PhysicsPrivate::CreateBatteryEntities(const EntityComponentManager &_ecm) void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) { // Assume the world will not be erased - // Only removing models is supported by ign-physics right now so we only + // Only removing models is supported by gz-physics right now so we only // remove links, joints and collisions if they are children of the removed // model. // We assume the links, joints and collisions will be removed from the @@ -1609,7 +1609,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) { if (!this->entityJointMap.HasEntity(_entity)) { - ignwarn << "Failed to find joint [" << _entity + gzwarn << "Failed to find joint [" << _entity << "]." << std::endl; return true; } @@ -1622,7 +1622,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting to detach a joint, but the physics " + gzdbg << "Attempting to detach a joint, but the physics " << "engine doesn't support feature " << "[DetachJointFeature]. Joint won't be detached." << std::endl; @@ -1633,7 +1633,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) return false; } - igndbg << "Detaching joint [" << _entity << "]" << std::endl; + gzdbg << "Detaching joint [" << _entity << "]" << std::endl; castEntity->Detach(); return true; }); @@ -1641,9 +1641,9 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) ////////////////////////////////////////////////// void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, - const ignition::gazebo::UpdateInfo &_info) + const gz::sim::UpdateInfo &_info) { - IGN_PROFILE("PhysicsPrivate::UpdatePhysics"); + GZ_PROFILE("PhysicsPrivate::UpdatePhysics"); // Battery state _ecm.Each( [&](const Entity & _entity, const components::BatterySoC *_bat) @@ -1705,7 +1705,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, if (jointVelocity.size() != jointPhys->GetDegreesOfFreedom()) { - ignwarn << "There is a mismatch in the degrees of freedom " + gzwarn << "There is a mismatch in the degrees of freedom " << "between Joint [" << _name->Data() << "(Entity=" << _entity << ")] and its JointVelocityReset " << "component. The joint has " @@ -1730,7 +1730,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, if (jointPosition.size() != jointPhys->GetDegreesOfFreedom()) { - ignwarn << "There is a mismatch in the degrees of freedom " + gzwarn << "There is a mismatch in the degrees of freedom " << "between Joint [" << _name->Data() << "(Entity=" << _entity << ")] and its JointPositionyReset " << "component. The joint has " @@ -1753,7 +1753,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, { if (force->Data().size() != jointPhys->GetDegreesOfFreedom()) { - ignwarn << "There is a mismatch in the degrees of freedom between " + gzwarn << "There is a mismatch in the degrees of freedom between " << "Joint [" << _name->Data() << "(Entity=" << _entity << ")] and its JointForceCmd component. The joint has " << jointPhys->GetDegreesOfFreedom() << " while the " @@ -1774,7 +1774,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, if (velReset) { - ignwarn << "Found both JointVelocityReset and " + gzwarn << "Found both JointVelocityReset and " << "JointVelocityCmd components for Joint [" << _name->Data() << "(Entity=" << _entity << "]). Ignoring JointVelocityCmd component." @@ -1784,7 +1784,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, if (velocityCmd.size() != jointPhys->GetDegreesOfFreedom()) { - ignwarn << "There is a mismatch in the degrees of freedom" + gzwarn << "There is a mismatch in the degrees of freedom" << " between Joint [" << _name->Data() << "(Entity=" << _entity<< ")] and its " << "JointVelocityCmd component. The joint has " @@ -1818,7 +1818,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, { if (!this->entityLinkMap.HasEntity(_entity)) { - ignwarn << "Failed to find link [" << _entity + gzwarn << "Failed to find link [" << _entity << "]." << std::endl; return true; } @@ -1830,7 +1830,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, static bool informed{false}; if (!informed) { - igndbg << "Attempting to apply a wrench, but the physics " + gzdbg << "Attempting to apply a wrench, but the physics " << "engine doesn't support feature " << "[AddLinkExternalForceTorque]. Wrench will be ignored." << std::endl; @@ -1859,7 +1859,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, { if (!this->entityLinkMap.HasEntity(_entity)) { - ignwarn << "Failed to find link [" << _entity << "]." + gzwarn << "Failed to find link [" << _entity << "]." << std::endl; return true; } @@ -1870,7 +1870,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, static bool informed{false}; if (!informed) { - igndbg << "Attempting to apply a wrench, but the physics " + gzdbg << "Attempting to apply a wrench, but the physics " << "engine doesn't support feature " << "[AddLinkExternalForceTorque]. Wrench will be ignored." << std::endl; @@ -1920,7 +1920,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, // world pose cmd currently not supported for nested models if (_entity != this->topLevelModelMap[_entity]) { - ignerr << "Unable to set world pose for nested models." + gzerr << "Unable to set world pose for nested models." << std::endl; return true; } @@ -1977,7 +1977,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, { if (!this->entityCollisionMap.HasEntity(_entity)) { - ignwarn << "Failed to find shape [" << _entity << "]." << std::endl; + gzwarn << "Failed to find shape [" << _entity << "]." << std::endl; return true; } @@ -1987,7 +1987,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, if (!slipComplianceShape) { - ignwarn << "Can't process Wheel Slip component, physics engine " + gzwarn << "Can't process Wheel Slip component, physics engine " << "missing SetShapeFrictionPyramidSlipCompliance" << std::endl; @@ -2018,7 +2018,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, // angular vel cmd currently not supported for nested models if (_entity != this->topLevelModelMap[_entity]) { - ignerr << "Unable to set angular velocity for nested models." + gzerr << "Unable to set angular velocity for nested models." << std::endl; return true; } @@ -2041,7 +2041,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, static bool informed{false}; if (!informed) { - igndbg << "Attempting to set model angular velocity, but the " + gzdbg << "Attempting to set model angular velocity, but the " << "physics engine doesn't support velocity commands. " << "Velocity won't be set." << std::endl; @@ -2068,7 +2068,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, // linear vel cmd currently not supported for nested models if (_entity != this->topLevelModelMap[_entity]) { - ignerr << "Unable to set linear velocity for nested models." + gzerr << "Unable to set linear velocity for nested models." << std::endl; return true; } @@ -2092,7 +2092,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, static bool informed{false}; if (!informed) { - igndbg << "Attempting to set model linear velocity, but the " + gzdbg << "Attempting to set model linear velocity, but the " << "physics engine doesn't support velocity commands. " << "Velocity won't be set." << std::endl; @@ -2114,7 +2114,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, { if (!this->entityLinkMap.HasEntity(_entity)) { - ignwarn << "Failed to find link [" << _entity + gzwarn << "Failed to find link [" << _entity << "]." << std::endl; return true; } @@ -2137,7 +2137,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, static bool informed{false}; if (!informed) { - igndbg << "Attempting to set link angular velocity, but the " + gzdbg << "Attempting to set link angular velocity, but the " << "physics engine doesn't support velocity commands. " << "Velocity won't be set." << std::endl; @@ -2166,7 +2166,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, { if (!this->entityLinkMap.HasEntity(_entity)) { - ignwarn << "Failed to find link [" << _entity + gzwarn << "Failed to find link [" << _entity << "]." << std::endl; return true; } @@ -2188,7 +2188,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, static bool informed{false}; if (!informed) { - igndbg << "Attempting to set link linear velocity, but the " + gzdbg << "Attempting to set link linear velocity, but the " << "physics engine doesn't support velocity commands. " << "Velocity won't be set." << std::endl; @@ -2221,7 +2221,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, { if (!this->entityModelMap.HasEntity(_entity)) { - ignwarn << "Failed to find model [" << _entity << "]." << std::endl; + gzwarn << "Failed to find model [" << _entity << "]." << std::endl; return true; } @@ -2233,7 +2233,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, static bool informed{false}; if (!informed) { - igndbg << "Attempting to get a bounding box, but the physics " + gzdbg << "Attempting to get a bounding box, but the physics " << "engine doesn't support feature " << "[GetModelBoundingBox]. Bounding box won't be populated." << std::endl; @@ -2256,13 +2256,13 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm, } ////////////////////////////////////////////////// -ignition::physics::ForwardStep::Output PhysicsPrivate::Step( +gz::physics::ForwardStep::Output PhysicsPrivate::Step( const std::chrono::steady_clock::duration &_dt) { - IGN_PROFILE("PhysicsPrivate::Step"); - ignition::physics::ForwardStep::Input input; - ignition::physics::ForwardStep::State state; - ignition::physics::ForwardStep::Output output; + GZ_PROFILE("PhysicsPrivate::Step"); + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; input.Get() = _dt; @@ -2275,7 +2275,7 @@ ignition::physics::ForwardStep::Output PhysicsPrivate::Step( } ////////////////////////////////////////////////// -ignition::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, +gz::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, const Entity &_to, const EntityComponentManager &_ecm) const { math::Pose3d transform; @@ -2313,31 +2313,31 @@ ignition::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, ////////////////////////////////////////////////// std::map PhysicsPrivate::ChangedLinks( EntityComponentManager &_ecm, - const ignition::physics::ForwardStep::Output &_updatedLinks) + const gz::physics::ForwardStep::Output &_updatedLinks) { - IGN_PROFILE("Links Frame Data"); + GZ_PROFILE("Links Frame Data"); std::map linkFrameData; // Check to see if the physics engine gave a list of changed poses. If not, we // will iterate through all of the links via the ECM to see which ones changed - if (_updatedLinks.Has()) + if (_updatedLinks.Has()) { for (const auto &link : - _updatedLinks.Query()->entries) + _updatedLinks.Query()->entries) { // get the gazebo entity that matches the updated physics link entity const auto linkPhys = this->entityLinkMap.GetPhysicsEntityPtr(link.body); if (nullptr == linkPhys) { - ignerr << "Internal error: a physics entity ptr with an ID of [" + gzerr << "Internal error: a physics entity ptr with an ID of [" << link.body << "] does not exist." << std::endl; continue; } auto entity = this->entityLinkMap.Get(linkPhys); if (entity == kNullEntity) { - ignerr << "Internal error: no gazebo entity matches the physics entity " + gzerr << "Internal error: no gazebo entity matches the physics entity " << "with ID [" << link.body << "]." << std::endl; continue; } @@ -2357,7 +2357,7 @@ std::map PhysicsPrivate::ChangedLinks( auto linkPhys = this->entityLinkMap.Get(_entity); if (nullptr == linkPhys) { - ignerr << "Internal error: link [" << _entity + gzerr << "Internal error: link [" << _entity << "] not in entity map" << std::endl; return true; } @@ -2367,7 +2367,7 @@ std::map PhysicsPrivate::ChangedLinks( // update the link pose if this is the first update, // or if the link pose has changed since the last update // (if the link pose hasn't changed, there's no need for a pose update) - const auto worldPoseMath3d = ignition::math::eigen3::convert( + const auto worldPoseMath3d = gz::math::eigen3::convert( frameData.pose); if ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end()) || !this->pose3Eql(this->linkWorldPoses[_entity], worldPoseMath3d)) @@ -2454,7 +2454,7 @@ void PhysicsPrivate::UpdateModelPose(const Entity _model, // once the model pose has been updated, all descendant link poses of this // model must be updated (whether the link actually changed pose or not) // since link poses are saved w.r.t. their parent model - auto model = gazebo::Model(_model); + auto model = gz::sim::Model(_model); for (const auto &childLink : model.Links(_ecm)) { // skip links that are already marked as a link to be updated @@ -2479,7 +2479,7 @@ void PhysicsPrivate::UpdateModelPose(const Entity _model, { auto staticComp = _ecm.Component(nestedModel); if (!staticComp || !staticComp->Data()) - ignerr << "Model [" << nestedModel << "] has no canonical link\n"; + gzerr << "Model [" << nestedModel << "] has no canonical link\n"; continue; } @@ -2508,7 +2508,7 @@ bool PhysicsPrivate::GetFrameDataRelativeToWorld(const Entity _entity, auto entityPhys = this->entityLinkMap.Get(_entity); if (nullptr == entityPhys) { - ignerr << "Internal error: entity [" << _entity + gzerr << "Internal error: entity [" << _entity << "] not in entity map" << std::endl; return false; } @@ -2520,9 +2520,9 @@ bool PhysicsPrivate::GetFrameDataRelativeToWorld(const Entity _entity, ////////////////////////////////////////////////// void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, std::map &_linkFrameData, - const ignition::gazebo::UpdateInfo &_info) + const gz::sim::UpdateInfo &_info) { - IGN_PROFILE("PhysicsPrivate::UpdateSim"); + GZ_PROFILE("PhysicsPrivate::UpdateSim"); // Populate world components with default values _ecm.EachNew( @@ -2560,7 +2560,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); - IGN_PROFILE_BEGIN("Models"); + GZ_PROFILE_BEGIN("Models"); // make sure we have an up-to-date mapping of canonical links to their models this->canonicalLinkModelTracker.AddNewModels(_ecm); @@ -2587,13 +2587,13 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, for (auto &modelEnt : canonicalLinkModels) this->UpdateModelPose(modelEnt, linkEntity, _ecm, _linkFrameData); } - IGN_PROFILE_END(); + GZ_PROFILE_END(); // Link poses, velocities... - IGN_PROFILE_BEGIN("Links"); + GZ_PROFILE_BEGIN("Links"); for (const auto &[entity, frameData] : _linkFrameData) { - IGN_PROFILE_BEGIN("Local pose"); + GZ_PROFILE_BEGIN("Local pose"); auto canonicalLink = _ecm.Component(entity); @@ -2606,7 +2606,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, auto parentModelPoseIt = this->modelWorldPoses.find(parentEntity); if (parentModelPoseIt == this->modelWorldPoses.end()) { - ignerr << "Internal error: parent model [" << parentEntity + gzerr << "Internal error: parent model [" << parentEntity << "] does not have a world pose available" << std::endl; continue; } @@ -2620,7 +2620,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, _ecm.SetChanged(entity, components::Pose::typeId, ComponentState::PeriodicChange); } - IGN_PROFILE_END(); + GZ_PROFILE_END(); // Populate world poses, velocities and accelerations of the link. For // now these components are updated only if another system has created @@ -2754,7 +2754,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, state); } } - IGN_PROFILE_END(); + GZ_PROFILE_END(); // history of applied joint forces (commands) _ecm.Each( @@ -2854,7 +2854,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); - ignition::math::Vector3d entityWorldAngularVel = + gz::math::Vector3d entityWorldAngularVel = math::eigen3::convert(entityFrameData.angularVelocity); auto entityBodyAngularVel = @@ -2879,7 +2879,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); - ignition::math::Vector3d entityWorldLinearAcc = + gz::math::Vector3d entityWorldLinearAcc = math::eigen3::convert(entityFrameData.linearAcceleration); auto entityBodyLinearAcc = @@ -2889,10 +2889,10 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); - IGN_PROFILE_END(); + GZ_PROFILE_END(); // Clear reset components - IGN_PROFILE_BEGIN("Clear / reset components"); + GZ_PROFILE_BEGIN("Clear / reset components"); std::vector entitiesPositionReset; _ecm.Each( [&](const Entity &_entity, components::JointPositionReset *) -> bool @@ -2947,7 +2947,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0); return true; }); - IGN_PROFILE_END(); + GZ_PROFILE_END(); _ecm.Each( [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool @@ -2964,7 +2964,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, }); // Update joint positions - IGN_PROFILE_BEGIN("Joints"); + GZ_PROFILE_BEGIN("Joints"); _ecm.Each( [&](const Entity &_entity, components::Joint *, components::JointPosition *_jointPos) -> bool @@ -3034,7 +3034,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); - IGN_PROFILE_END(); + GZ_PROFILE_END(); // Update joint transmitteds _ecm.Each( @@ -3065,7 +3065,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, static bool informed{false}; if (!informed) { - igndbg + gzdbg << "Attempting to get joint transmitted wrenches, but the " "physics engine doesn't support this feature. Values in the " "JointTransmittedWrench component will not be meaningful." @@ -3083,7 +3083,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, ////////////////////////////////////////////////// void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) { - IGN_PROFILE("PhysicsPrivate::UpdateCollisions"); + GZ_PROFILE("PhysicsPrivate::UpdateCollisions"); // Quit early if the ContactData component hasn't been created. This means // there are no systems that need contact information if (!_ecm.HasComponentType(components::ContactSensorData::typeId)) @@ -3113,13 +3113,13 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) if (worldEntity == kNullEntity) { - ignerr << "Missing world entity.\n"; + gzerr << "Missing world entity.\n"; return; } if (!this->entityWorldMap.HasEntity(worldEntity)) { - ignwarn << "Failed to find world [" << worldEntity << "]." << std::endl; + gzwarn << "Failed to find world [" << worldEntity << "]." << std::endl; return; } @@ -3130,7 +3130,7 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) static bool informed{false}; if (!informed) { - igndbg << "Attempting process contacts, but the physics " + gzdbg << "Attempting process contacts, but the physics " << "engine doesn't support contact features. " << "Contacts won't be computed." << std::endl; @@ -3146,7 +3146,7 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) const WorldShapeType::ExtraContactData *extra; }; - // Each contact object we get from ign-physics contains the EntityPtrs of the + // Each contact object we get from gz-physics contains the EntityPtrs of the // two colliding entities and other data about the contact such as the // position. This map groups contacts so that it is easy to query all the // contacts of one entity. @@ -3310,10 +3310,10 @@ physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset( return this->engine->Resolve(relFrameData, physics::FrameID::World()); } -IGNITION_ADD_PLUGIN(Physics, - ignition::gazebo::System, +GZ_ADD_PLUGIN(Physics, + gz::sim::System, Physics::ISystemConfigure, Physics::ISystemUpdate) -IGNITION_ADD_PLUGIN_ALIAS(Physics, "ignition::gazebo::systems::Physics") -IGNITION_ADD_PLUGIN_ALIAS(Physics, "scenario::plugins::gazebo::Physics") +GZ_ADD_PLUGIN_ALIAS(Physics, "gz::sim::systems::Physics") +GZ_ADD_PLUGIN_ALIAS(Physics, "scenario::plugins::gazebo::Physics") diff --git a/scenario/src/plugins/Physics/Physics.hh b/scenario/src/plugins/Physics/Physics.hh index 057fe71b3..013fb9297 100644 --- a/scenario/src/plugins/Physics/Physics.hh +++ b/scenario/src/plugins/Physics/Physics.hh @@ -14,56 +14,56 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_PHYSICS_HH_ -#define IGNITION_GAZEBO_SYSTEMS_PHYSICS_HH_ +#ifndef GZ_SIM_SYSTEMS_PHYSICS_HH_ +#define GZ_SIM_SYSTEMS_PHYSICS_HH_ // clang-format off #include #include #include -#include -#include +#include +#include // Features need to be defined ahead of entityCast -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include +#include +#include -namespace ignition +namespace gz { -namespace gazebo +namespace sim { // Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { // Forward declarations. class PhysicsPrivate; - /// \class Physics Physics.hh ignition/gazebo/systems/Physics.hh + /// \class Physics Physics.hh gz/sim/systems/Physics.hh /// \brief Base class for a System. class Physics: public System, From 3cf66d9d9766beaf687e93be685b7a5d95294ab5 Mon Sep 17 00:00:00 2001 From: Andrea Ostuni Date: Sun, 24 Dec 2023 21:03:36 +0100 Subject: [PATCH 2/5] use new sdf::Plugin API to load plugins --- scenario/bindings/__init__.py | 4 ++-- .../gazebo/include/scenario/gazebo/helpers.h | 3 --- scenario/src/gazebo/src/GazeboSimulator.cpp | 2 +- scenario/src/gazebo/src/World.cpp | 2 +- scenario/src/gazebo/src/helpers.cpp | 19 ------------------- scenario/src/gazebo/src/utils.cpp | 18 +++++++++--------- scenario/src/plugins/Physics/Physics.cc | 7 ++++--- 7 files changed, 17 insertions(+), 38 deletions(-) diff --git a/scenario/bindings/__init__.py b/scenario/bindings/__init__.py index 4b0c34bb0..5ad48a04a 100644 --- a/scenario/bindings/__init__.py +++ b/scenario/bindings/__init__.py @@ -13,7 +13,7 @@ def supported_versions_specifier_set() -> packaging.specifiers.SpecifierSet: - # If 6 is the Gazebo distribution major version, the following specifier enables + # If 7 is the Gazebo distribution major version, the following specifier enables # the compatibility with all the following versions: # # 6.Y.Z.devK @@ -23,7 +23,7 @@ def supported_versions_specifier_set() -> packaging.specifiers.SpecifierSet: # 6.Y.Z.preK # 6.Y.Z.postK # - return packaging.specifiers.SpecifierSet(">=6.0.0.pre,<7.0.0.dev") + return packaging.specifiers.SpecifierSet(">=7.0.0.pre,<8.0.0.dev") class InstallMode(Enum): diff --git a/scenario/src/gazebo/include/scenario/gazebo/helpers.h b/scenario/src/gazebo/include/scenario/gazebo/helpers.h index f22d80c19..428d423dc 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/helpers.h +++ b/scenario/src/gazebo/include/scenario/gazebo/helpers.h @@ -157,9 +157,6 @@ namespace scenario::gazebo::utils { const double realTimeUpdateRate, const size_t worldIndex = 0); - sdf::ElementPtr getPluginSDFElement(const std::string& libName, - const std::string& className); - sdf::JointType toSdf(const scenario::core::JointType type); scenario::core::JointType fromSdf(const sdf::JointType sdfType); diff --git a/scenario/src/gazebo/src/GazeboSimulator.cpp b/scenario/src/gazebo/src/GazeboSimulator.cpp index 257b2106a..e3d157f2f 100644 --- a/scenario/src/gazebo/src/GazeboSimulator.cpp +++ b/scenario/src/gazebo/src/GazeboSimulator.cpp @@ -339,7 +339,7 @@ bool GazeboSimulator::gui(const int verbosity) sDebug << "Starting the SceneBroadcaster plugin" << std::endl; auto world = this->getWorld(worldName); if (!std::static_pointer_cast(world)->insertWorldPlugin( - "gz-sim-scene-broadcaster-system", + "libgz-sim-scene-broadcaster-system.so", "gz::sim::systems::SceneBroadcaster")) { sError << "Failed to load SceneBroadcaster plugin" << std::endl; return false; diff --git a/scenario/src/gazebo/src/World.cpp b/scenario/src/gazebo/src/World.cpp index 0b682e075..72eeb673e 100644 --- a/scenario/src/gazebo/src/World.cpp +++ b/scenario/src/gazebo/src/World.cpp @@ -271,7 +271,7 @@ bool World::setPhysicsEngine(const PhysicsEngine engine) m_ecm, m_entity, pluginLib); // Vendored Physics system - const std::string libName = "PhysicsSystem"; + const std::string libName = "libPhysicsSystem.so"; const std::string className = "scenario::plugins::gazebo::Physics"; // Load the Physics system diff --git a/scenario/src/gazebo/src/helpers.cpp b/scenario/src/gazebo/src/helpers.cpp index b0172e4fe..77baffde0 100644 --- a/scenario/src/gazebo/src/helpers.cpp +++ b/scenario/src/gazebo/src/helpers.cpp @@ -428,25 +428,6 @@ bool utils::updateSDFPhysics(sdf::Root& sdfRoot, return true; } -sdf::ElementPtr utils::getPluginSDFElement(const std::string& libName, - const std::string& className) -{ - // Create the plugin SDF element - auto pluginElement = std::make_shared(); - - // Initialize the attributes - pluginElement->SetName("plugin"); - pluginElement->AddAttribute( - "name", "string", className, true, "plugin name"); - pluginElement->AddAttribute( - "filename", "string", libName, true, "pluginfilename"); - - // Create the plugin description - pluginElement->AddElementDescription(pluginElement->Clone()); - - return pluginElement; -} - sdf::JointType utils::toSdf(const scenario::core::JointType type) { sdf::JointType sdfType; diff --git a/scenario/src/gazebo/src/utils.cpp b/scenario/src/gazebo/src/utils.cpp index 9a031dc10..51009ea1b 100644 --- a/scenario/src/gazebo/src/utils.cpp +++ b/scenario/src/gazebo/src/utils.cpp @@ -39,10 +39,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include @@ -64,7 +66,7 @@ std::string utils::findSdfFile(const std::string& fileName) gz::common::SystemPaths systemPaths; systemPaths.SetFilePathEnv("GZ_SIM_RESOURCE_PATH"); - systemPaths.AddFilePaths(GZ_SIM_WORLD_INSTALL_DIR); + systemPaths.AddFilePaths(gz::sim::getWorldInstallDir()); // Find the file std::string sdfFilePath = systemPaths.FindFile(fileName); @@ -390,8 +392,7 @@ bool utils::insertPluginToGazeboEntity(const GazeboEntity& gazeboEntity, << gazeboEntity.entity() << "]" << std::endl; // Create a new element without context - sdf::ElementPtr pluginElement = - utils::getPluginSDFElement(libName, className); + auto pluginElement = sdf::Plugin(libName, className); // Insert the context into the plugin element if (!context.empty()) { @@ -409,18 +410,17 @@ bool utils::insertPluginToGazeboEntity(const GazeboEntity& gazeboEntity, // Insert the plugin context elements while (contextNextElement) { - pluginElement->InsertElement(contextNextElement); + pluginElement.InsertContent(contextNextElement); contextNextElement = contextNextElement->GetNextElement(); } } - // The plugin element must be wrapped in another element, otherwise - // who receives it does not get the additional context - const auto wrapped = sdf::SDF::WrapInRoot(pluginElement); + auto pluginElements = std::vector(); + pluginElements.push_back(pluginElement); // Trigger the plugin loading - gazeboEntity.eventManager()->Emit( - gazeboEntity.entity(), wrapped); + gazeboEntity.eventManager()->Emit( + gazeboEntity.entity(), pluginElements); return true; } diff --git a/scenario/src/plugins/Physics/Physics.cc b/scenario/src/plugins/Physics/Physics.cc index ad76baae2..8f333d9c6 100644 --- a/scenario/src/plugins/Physics/Physics.cc +++ b/scenario/src/plugins/Physics/Physics.cc @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -672,7 +673,7 @@ void Physics::Configure(const Entity &_entity, // * Engines installed with gz-physics common::SystemPaths systemPaths; systemPaths.SetPluginPathEnv(this->dataPtr->pluginPathEnv); - systemPaths.AddPluginPaths({GZ_PHYSICS_ENGINE_INSTALL_DIR}); + systemPaths.AddPluginPaths(gz::physics::getEngineInstallDir()); auto pathToLib = systemPaths.FindSharedLibrary(pluginLib); if (pathToLib.empty()) @@ -1388,8 +1389,8 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) joint.SetRawPose(_pose->Data()); joint.SetThreadPitch(_threadPitch->Data()); - joint.SetParentLinkName(_parentLinkName->Data()); - joint.SetChildLinkName(_childLinkName->Data()); + joint.SetParentName(_parentLinkName->Data()); + joint.SetChildName(_childLinkName->Data()); auto jointAxis = _ecm.Component(_entity); auto jointAxis2 = _ecm.Component(_entity); From 094b815fddbaecd0b88460fe8244c4b1a0d81c0b Mon Sep 17 00:00:00 2001 From: Andrea Ostuni Date: Sat, 30 Dec 2023 00:52:01 +0100 Subject: [PATCH 3/5] port to gymnasium v.0.29 --- docs/sphinx/CMakeLists.txt | 16 ++++---- .../gym_ignition_environments.models.rst | 20 ++++----- .../gym_ignition_environments.randomizers.rst | 12 +++--- .../gym_ignition_environments.rst | 10 ++--- .../gym_ignition_environments.tasks.rst | 20 ++++----- .../apidoc/gym-ignition/gym_ignition.base.rst | 12 +++--- .../gym_ignition.context.gazebo.rst | 12 +++--- .../gym-ignition/gym_ignition.context.rst | 6 +-- .../gym_ignition.randomizers.model.rst | 8 ++-- .../gym_ignition.randomizers.physics.rst | 8 ++-- .../gym-ignition/gym_ignition.randomizers.rst | 16 ++++---- .../gym_ignition.rbd.idyntree.rst | 20 ++++----- .../apidoc/gym-ignition/gym_ignition.rbd.rst | 14 +++---- .../apidoc/gym-ignition/gym_ignition.rst | 18 ++++---- .../gym-ignition/gym_ignition.runtimes.rst | 12 +++--- .../gym-ignition/gym_ignition.scenario.rst | 12 +++--- .../gym-ignition/gym_ignition.utils.rst | 28 ++++++------- docs/sphinx/conf.py | 2 +- docs/sphinx/getting_started/scenario.rst | 2 +- docs/sphinx/index.rst | 20 ++++----- .../installation/system_configuration.rst | 2 +- examples/panda_pick_and_place.py | 28 ++++++------- examples/python/launch_cartpole.py | 16 ++++---- python/{gym_ignition => gym_gz}/__init__.py | 20 ++++----- .../{gym_ignition => gym_gz}/base/__init__.py | 0 .../{gym_ignition => gym_gz}/base/runtime.py | 25 +++++------ python/{gym_ignition => gym_gz}/base/task.py | 34 +++++++++++---- .../context/__init__.py | 0 .../context/gazebo/__init__.py | 0 .../context/gazebo/controllers.py | 2 +- .../context/gazebo/plugin.py | 4 +- .../randomizers/__init__.py | 0 .../randomizers/abc.py | 16 ++++---- .../randomizers/gazebo_env_randomizer.py | 35 ++++++++-------- .../randomizers/model/__init__.py | 0 .../randomizers/model/sdf.py | 6 +-- .../randomizers/physics/__init__.py | 0 .../randomizers/physics/dart.py | 6 +-- .../{gym_ignition => gym_gz}/rbd/__init__.py | 0 .../rbd/conversions.py | 0 .../rbd/idyntree/__init__.py | 0 .../rbd/idyntree/helpers.py | 2 +- .../rbd/idyntree/inverse_kinematics_nlp.py | 2 +- .../rbd/idyntree/kindyncomputations.py | 4 +- .../rbd/idyntree/numpy.py | 2 +- python/{gym_ignition => gym_gz}/rbd/utils.py | 0 .../runtimes/__init__.py | 0 .../runtimes/gazebo_runtime.py | 41 +++++++++++-------- .../runtimes/realtime_runtime.py | 17 ++++---- .../scenario/__init__.py | 0 .../scenario/model_with_file.py | 0 .../scenario/model_wrapper.py | 0 .../utils/__init__.py | 0 .../{gym_ignition => gym_gz}/utils/logger.py | 16 ++++---- python/{gym_ignition => gym_gz}/utils/math.py | 0 python/{gym_ignition => gym_gz}/utils/misc.py | 0 .../utils/resource_finder.py | 18 ++++---- .../utils/scenario.py | 4 +- .../{gym_ignition => gym_gz}/utils/typing.py | 5 ++- .../__init__.py | 12 +++--- .../models/__init__.py | 0 .../models/cartpole.py | 8 ++-- .../models/icub.py | 12 +++--- .../models/panda.py | 8 ++-- .../models/pendulum.py | 8 ++-- .../randomizers/__init__.py | 0 .../randomizers/cartpole.py | 14 +++---- .../randomizers/cartpole_no_rand.py | 10 ++--- .../tasks/__init__.py | 0 .../tasks/cartpole_continuous_balancing.py | 24 ++++++----- .../tasks/cartpole_continuous_swingup.py | 21 ++++++---- .../tasks/cartpole_discrete_balancing.py | 21 ++++++---- .../tasks/pendulum_swingup.py | 21 ++++++---- scenario/bindings/__init__.py | 2 +- scenario/setup.cfg | 2 +- .../gazebo/include/scenario/gazebo/utils.h | 2 +- scenario/src/gazebo/src/utils.cpp | 1 + setup.cfg | 14 +++---- .../test_environments_gazebowrapper.py | 8 ++-- tests/.python/test_joint_force.py | 2 +- .../.python/test_pendulum_wrt_ground_truth.py | 34 +++++++-------- tests/.python/test_rates.py | 4 +- tests/.python/test_robot_controller.py | 12 +++--- tests/.python/test_runtimes.py | 38 ++++++++--------- tests/.python/utils.py | 8 ++-- tests/assets/worlds/fuel_support.sdf | 8 ++-- tests/common/utils.py | 6 +-- .../__init__.py | 0 .../test_inverse_kinematics.py | 8 ++-- .../test_normalization.py | 4 +- .../test_reproducibility.py | 30 ++++++++------ .../test_sdf_randomizer.py | 18 ++++---- tests/test_scenario/test_contacts.py | 8 ++-- .../test_scenario/test_custom_controllers.py | 6 +-- tests/test_scenario/test_gazebo_simulator.py | 4 +- ...{test_ignition_fuel.py => test_gz_fuel.py} | 2 +- tests/test_scenario/test_link_velocities.py | 9 ++-- tests/test_scenario/test_model.py | 36 ++++++++-------- tests/test_scenario/test_multi_world.py | 6 +-- tests/test_scenario/test_pid_controllers.py | 4 +- tests/test_scenario/test_velocity_direct.py | 4 +- 101 files changed, 531 insertions(+), 481 deletions(-) rename python/{gym_ignition => gym_gz}/__init__.py (65%) rename python/{gym_ignition => gym_gz}/base/__init__.py (100%) rename python/{gym_ignition => gym_gz}/base/runtime.py (72%) rename python/{gym_ignition => gym_gz}/base/task.py (87%) rename python/{gym_ignition => gym_gz}/context/__init__.py (100%) rename python/{gym_ignition => gym_gz}/context/gazebo/__init__.py (100%) rename python/{gym_ignition => gym_gz}/context/gazebo/controllers.py (96%) rename python/{gym_ignition => gym_gz}/context/gazebo/plugin.py (93%) rename python/{gym_ignition => gym_gz}/randomizers/__init__.py (100%) rename python/{gym_ignition => gym_gz}/randomizers/abc.py (85%) rename python/{gym_ignition => gym_gz}/randomizers/gazebo_env_randomizer.py (79%) rename python/{gym_ignition => gym_gz}/randomizers/model/__init__.py (100%) rename python/{gym_ignition => gym_gz}/randomizers/model/sdf.py (97%) rename python/{gym_ignition => gym_gz}/randomizers/physics/__init__.py (100%) rename python/{gym_ignition => gym_gz}/randomizers/physics/dart.py (80%) rename python/{gym_ignition => gym_gz}/rbd/__init__.py (100%) rename python/{gym_ignition => gym_gz}/rbd/conversions.py (100%) rename python/{gym_ignition => gym_gz}/rbd/idyntree/__init__.py (100%) rename python/{gym_ignition => gym_gz}/rbd/idyntree/helpers.py (98%) rename python/{gym_ignition => gym_gz}/rbd/idyntree/inverse_kinematics_nlp.py (99%) rename python/{gym_ignition => gym_gz}/rbd/idyntree/kindyncomputations.py (99%) rename python/{gym_ignition => gym_gz}/rbd/idyntree/numpy.py (99%) rename python/{gym_ignition => gym_gz}/rbd/utils.py (100%) rename python/{gym_ignition => gym_gz}/runtimes/__init__.py (100%) rename python/{gym_ignition => gym_gz}/runtimes/gazebo_runtime.py (89%) rename python/{gym_ignition => gym_gz}/runtimes/realtime_runtime.py (76%) rename python/{gym_ignition => gym_gz}/scenario/__init__.py (100%) rename python/{gym_ignition => gym_gz}/scenario/model_with_file.py (100%) rename python/{gym_ignition => gym_gz}/scenario/model_wrapper.py (100%) rename python/{gym_ignition => gym_gz}/utils/__init__.py (100%) rename python/{gym_ignition => gym_gz}/utils/logger.py (89%) rename python/{gym_ignition => gym_gz}/utils/math.py (100%) rename python/{gym_ignition => gym_gz}/utils/misc.py (100%) rename python/{gym_ignition => gym_gz}/utils/resource_finder.py (86%) rename python/{gym_ignition => gym_gz}/utils/scenario.py (96%) rename python/{gym_ignition => gym_gz}/utils/typing.py (74%) rename python/{gym_ignition_environments => gym_gz_environments}/__init__.py (78%) rename python/{gym_ignition_environments => gym_gz_environments}/models/__init__.py (100%) rename python/{gym_ignition_environments => gym_gz_environments}/models/cartpole.py (85%) rename python/{gym_ignition_environments => gym_gz_environments}/models/icub.py (92%) rename python/{gym_ignition_environments => gym_gz_environments}/models/panda.py (92%) rename python/{gym_ignition_environments => gym_gz_environments}/models/pendulum.py (85%) rename python/{gym_ignition_environments => gym_gz_environments}/randomizers/__init__.py (100%) rename python/{gym_ignition_environments => gym_gz_environments}/randomizers/cartpole.py (93%) rename python/{gym_ignition_environments => gym_gz_environments}/randomizers/cartpole_no_rand.py (86%) rename python/{gym_ignition_environments => gym_gz_environments}/tasks/__init__.py (100%) rename python/{gym_ignition_environments => gym_gz_environments}/tasks/cartpole_continuous_balancing.py (90%) rename python/{gym_ignition_environments => gym_gz_environments}/tasks/cartpole_continuous_swingup.py (92%) rename python/{gym_ignition_environments => gym_gz_environments}/tasks/cartpole_discrete_balancing.py (90%) rename python/{gym_ignition_environments => gym_gz_environments}/tasks/pendulum_swingup.py (90%) rename tests/{test_gym_ignition => test_gym_gz}/__init__.py (100%) rename tests/{test_gym_ignition => test_gym_gz}/test_inverse_kinematics.py (95%) rename tests/{test_gym_ignition => test_gym_gz}/test_normalization.py (91%) rename tests/{test_gym_ignition => test_gym_gz}/test_reproducibility.py (64%) rename tests/{test_gym_ignition => test_gym_gz}/test_sdf_randomizer.py (95%) rename tests/test_scenario/{test_ignition_fuel.py => test_gz_fuel.py} (95%) diff --git a/docs/sphinx/CMakeLists.txt b/docs/sphinx/CMakeLists.txt index 5bdcf3a54..935889b58 100644 --- a/docs/sphinx/CMakeLists.txt +++ b/docs/sphinx/CMakeLists.txt @@ -22,8 +22,8 @@ set(BINDINGS_MODULES_DIR "${PROJECT_BINARY_DIR}/scenario/bindings") # The files in the following folders are replaced: # # - docs/sphinx/scenario -# - docs/sphinx/gym_ignition -# - docs/sphinx/gym_ignition_environments +# - docs/sphinx/gym_gz +# - docs/sphinx/gym_gz_environments if (NOT TARGET ScenarioSwig::Core) message(FATAL_ERROR "Target ScenarioSwig::Core not found") @@ -43,9 +43,9 @@ add_custom_command( ${SPHINX_APIDOC_EXECUTABLE} --force --no-toc --module-first --maxdepth 4 -t ${CMAKE_CURRENT_SOURCE_DIR}/_templates - -o ${CMAKE_CURRENT_SOURCE_DIR}/apidoc/gym-ignition - ${PYTHON_PACKAGES_DIR}/gym_ignition - COMMENT "Building apidoc") + -o ${CMAKE_CURRENT_SOURCE_DIR}/apidoc/gym-gz + ${PYTHON_PACKAGES_DIR}/gym_gz + COMMENT "Building apidoc") add_custom_command( TARGET apidoc POST_BUILD @@ -53,9 +53,9 @@ add_custom_command( ${SPHINX_APIDOC_EXECUTABLE} --force --no-toc --module-first --maxdepth 4 -t ${CMAKE_CURRENT_SOURCE_DIR}/_templates - -o ${CMAKE_CURRENT_SOURCE_DIR}/apidoc/gym-ignition-environments - ${PYTHON_PACKAGES_DIR}/gym_ignition_environments - COMMENT "Building apidoc") + -o ${CMAKE_CURRENT_SOURCE_DIR}/apidoc/gym-gz-environments + ${PYTHON_PACKAGES_DIR}/gym_gz_environments + COMMENT "Building apidoc") add_custom_command( TARGET apidoc POST_BUILD diff --git a/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.models.rst b/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.models.rst index fb0931970..24df9c0e6 100644 --- a/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.models.rst +++ b/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.models.rst @@ -1,40 +1,40 @@ -gym\_ignition\_environments.models +gym\_gz\_environments.models ================================== -.. automodule:: gym_ignition_environments.models +.. automodule:: gym_gz_environments.models :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.models.cartpole +gym\_gz\_environments.models.cartpole ------------------------------------------- -.. automodule:: gym_ignition_environments.models.cartpole +.. automodule:: gym_gz_environments.models.cartpole :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.models.icub +gym\_gz\_environments.models.icub --------------------------------------- -.. automodule:: gym_ignition_environments.models.icub +.. automodule:: gym_gz_environments.models.icub :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.models.panda +gym\_gz\_environments.models.panda ---------------------------------------- -.. automodule:: gym_ignition_environments.models.panda +.. automodule:: gym_gz_environments.models.panda :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.models.pendulum +gym\_gz\_environments.models.pendulum ------------------------------------------- -.. automodule:: gym_ignition_environments.models.pendulum +.. automodule:: gym_gz_environments.models.pendulum :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.randomizers.rst b/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.randomizers.rst index 9d6a2ae6f..dcb12b423 100644 --- a/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.randomizers.rst +++ b/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.randomizers.rst @@ -1,24 +1,24 @@ -gym\_ignition\_environments.randomizers +gym\_gz\_environments.randomizers ======================================= -.. automodule:: gym_ignition_environments.randomizers +.. automodule:: gym_gz_environments.randomizers :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.randomizers.cartpole +gym\_gz\_environments.randomizers.cartpole ------------------------------------------------ -.. automodule:: gym_ignition_environments.randomizers.cartpole +.. automodule:: gym_gz_environments.randomizers.cartpole :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.randomizers.cartpole\_no\_rand +gym\_gz\_environments.randomizers.cartpole\_no\_rand ---------------------------------------------------------- -.. automodule:: gym_ignition_environments.randomizers.cartpole_no_rand +.. automodule:: gym_gz_environments.randomizers.cartpole_no_rand :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.rst b/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.rst index c343433cf..358406a65 100644 --- a/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.rst +++ b/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.rst @@ -1,7 +1,7 @@ -gym\_ignition\_environments +gym\_gz\_environments =========================== -.. automodule:: gym_ignition_environments +.. automodule:: gym_gz_environments :members: :undoc-members: :show-inheritance: @@ -10,6 +10,6 @@ gym\_ignition\_environments .. toctree:: :maxdepth: 4 - gym_ignition_environments.models - gym_ignition_environments.randomizers - gym_ignition_environments.tasks + gym_gz_environments.models + gym_gz_environments.randomizers + gym_gz_environments.tasks diff --git a/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.tasks.rst b/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.tasks.rst index 8d9243a3f..e59913404 100644 --- a/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.tasks.rst +++ b/docs/sphinx/apidoc/gym-ignition-environments/gym_ignition_environments.tasks.rst @@ -1,40 +1,40 @@ -gym\_ignition\_environments.tasks +gym\_gz\_environments.tasks ================================= -.. automodule:: gym_ignition_environments.tasks +.. automodule:: gym_gz_environments.tasks :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.tasks.cartpole\_continuous\_balancing +gym\_gz\_environments.tasks.cartpole\_continuous\_balancing ----------------------------------------------------------------- -.. automodule:: gym_ignition_environments.tasks.cartpole_continuous_balancing +.. automodule:: gym_gz_environments.tasks.cartpole_continuous_balancing :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.tasks.cartpole\_continuous\_swingup +gym\_gz\_environments.tasks.cartpole\_continuous\_swingup --------------------------------------------------------------- -.. automodule:: gym_ignition_environments.tasks.cartpole_continuous_swingup +.. automodule:: gym_gz_environments.tasks.cartpole_continuous_swingup :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.tasks.cartpole\_discrete\_balancing +gym\_gz\_environments.tasks.cartpole\_discrete\_balancing --------------------------------------------------------------- -.. automodule:: gym_ignition_environments.tasks.cartpole_discrete_balancing +.. automodule:: gym_gz_environments.tasks.cartpole_discrete_balancing :members: :undoc-members: :show-inheritance: -gym\_ignition\_environments.tasks.pendulum\_swingup +gym\_gz\_environments.tasks.pendulum\_swingup --------------------------------------------------- -.. automodule:: gym_ignition_environments.tasks.pendulum_swingup +.. automodule:: gym_gz_environments.tasks.pendulum_swingup :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.base.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.base.rst index ce032008f..927be5dc7 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.base.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.base.rst @@ -1,24 +1,24 @@ -gym\_ignition.base +gym\_gz.base ================== -.. automodule:: gym_ignition.base +.. automodule:: gym_gz.base :members: :undoc-members: :show-inheritance: -gym\_ignition.base.runtime +gym\_gz.base.runtime -------------------------- -.. automodule:: gym_ignition.base.runtime +.. automodule:: gym_gz.base.runtime :members: :undoc-members: :show-inheritance: -gym\_ignition.base.task +gym\_gz.base.task ----------------------- -.. automodule:: gym_ignition.base.task +.. automodule:: gym_gz.base.task :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.context.gazebo.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.context.gazebo.rst index fd0214131..377e12f70 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.context.gazebo.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.context.gazebo.rst @@ -1,24 +1,24 @@ -gym\_ignition.context.gazebo +gym\_gz.context.gazebo ============================ -.. automodule:: gym_ignition.context.gazebo +.. automodule:: gym_gz.context.gazebo :members: :undoc-members: :show-inheritance: -gym\_ignition.context.gazebo.controllers +gym\_gz.context.gazebo.controllers ---------------------------------------- -.. automodule:: gym_ignition.context.gazebo.controllers +.. automodule:: gym_gz.context.gazebo.controllers :members: :undoc-members: :show-inheritance: -gym\_ignition.context.gazebo.plugin +gym\_gz.context.gazebo.plugin ----------------------------------- -.. automodule:: gym_ignition.context.gazebo.plugin +.. automodule:: gym_gz.context.gazebo.plugin :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.context.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.context.rst index 623fc7b78..997601db5 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.context.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.context.rst @@ -1,7 +1,7 @@ -gym\_ignition.context +gym\_gz.context ===================== -.. automodule:: gym_ignition.context +.. automodule:: gym_gz.context :members: :undoc-members: :show-inheritance: @@ -10,4 +10,4 @@ gym\_ignition.context .. toctree:: :maxdepth: 4 - gym_ignition.context.gazebo + gym_gz.context.gazebo diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.model.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.model.rst index e0d4aed18..ece3d7b36 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.model.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.model.rst @@ -1,16 +1,16 @@ -gym\_ignition.randomizers.model +gym\_gz.randomizers.model =============================== -.. automodule:: gym_ignition.randomizers.model +.. automodule:: gym_gz.randomizers.model :members: :undoc-members: :show-inheritance: -gym\_ignition.randomizers.model.sdf +gym\_gz.randomizers.model.sdf ----------------------------------- -.. automodule:: gym_ignition.randomizers.model.sdf +.. automodule:: gym_gz.randomizers.model.sdf :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.physics.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.physics.rst index 9932ec46c..b6f240bb0 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.physics.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.physics.rst @@ -1,16 +1,16 @@ -gym\_ignition.randomizers.physics +gym\_gz.randomizers.physics ================================= -.. automodule:: gym_ignition.randomizers.physics +.. automodule:: gym_gz.randomizers.physics :members: :undoc-members: :show-inheritance: -gym\_ignition.randomizers.physics.dart +gym\_gz.randomizers.physics.dart -------------------------------------- -.. automodule:: gym_ignition.randomizers.physics.dart +.. automodule:: gym_gz.randomizers.physics.dart :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.rst index 47bc4b37b..6ba046d56 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.randomizers.rst @@ -1,7 +1,7 @@ -gym\_ignition.randomizers +gym\_gz.randomizers ========================= -.. automodule:: gym_ignition.randomizers +.. automodule:: gym_gz.randomizers :members: :undoc-members: :show-inheritance: @@ -10,22 +10,22 @@ gym\_ignition.randomizers .. toctree:: :maxdepth: 4 - gym_ignition.randomizers.model - gym_ignition.randomizers.physics + gym_gz.randomizers.model + gym_gz.randomizers.physics -gym\_ignition.randomizers.abc +gym\_gz.randomizers.abc ----------------------------- -.. automodule:: gym_ignition.randomizers.abc +.. automodule:: gym_gz.randomizers.abc :members: :undoc-members: :show-inheritance: -gym\_ignition.randomizers.gazebo\_env\_randomizer +gym\_gz.randomizers.gazebo\_env\_randomizer ------------------------------------------------- -.. automodule:: gym_ignition.randomizers.gazebo_env_randomizer +.. automodule:: gym_gz.randomizers.gazebo_env_randomizer :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.rbd.idyntree.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.rbd.idyntree.rst index 926769242..bb3223cf3 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.rbd.idyntree.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.rbd.idyntree.rst @@ -1,40 +1,40 @@ -gym\_ignition.rbd.idyntree +gym\_gz.rbd.idyntree ========================== -.. automodule:: gym_ignition.rbd.idyntree +.. automodule:: gym_gz.rbd.idyntree :members: :undoc-members: :show-inheritance: -gym\_ignition.rbd.idyntree.helpers +gym\_gz.rbd.idyntree.helpers ---------------------------------- -.. automodule:: gym_ignition.rbd.idyntree.helpers +.. automodule:: gym_gz.rbd.idyntree.helpers :members: :undoc-members: :show-inheritance: -gym\_ignition.rbd.idyntree.inverse\_kinematics\_nlp +gym\_gz.rbd.idyntree.inverse\_kinematics\_nlp --------------------------------------------------- -.. automodule:: gym_ignition.rbd.idyntree.inverse_kinematics_nlp +.. automodule:: gym_gz.rbd.idyntree.inverse_kinematics_nlp :members: :undoc-members: :show-inheritance: -gym\_ignition.rbd.idyntree.kindyncomputations +gym\_gz.rbd.idyntree.kindyncomputations --------------------------------------------- -.. automodule:: gym_ignition.rbd.idyntree.kindyncomputations +.. automodule:: gym_gz.rbd.idyntree.kindyncomputations :members: :undoc-members: :show-inheritance: -gym\_ignition.rbd.idyntree.numpy +gym\_gz.rbd.idyntree.numpy -------------------------------- -.. automodule:: gym_ignition.rbd.idyntree.numpy +.. automodule:: gym_gz.rbd.idyntree.numpy :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.rbd.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.rbd.rst index d3d328c0f..54a998b15 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.rbd.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.rbd.rst @@ -1,7 +1,7 @@ -gym\_ignition.rbd +gym\_gz.rbd ================= -.. automodule:: gym_ignition.rbd +.. automodule:: gym_gz.rbd :members: :undoc-members: :show-inheritance: @@ -10,21 +10,21 @@ gym\_ignition.rbd .. toctree:: :maxdepth: 4 - gym_ignition.rbd.idyntree + gym_gz.rbd.idyntree -gym\_ignition.rbd.conversions +gym\_gz.rbd.conversions ----------------------------- -.. automodule:: gym_ignition.rbd.conversions +.. automodule:: gym_gz.rbd.conversions :members: :undoc-members: :show-inheritance: -gym\_ignition.rbd.utils +gym\_gz.rbd.utils ----------------------- -.. automodule:: gym_ignition.rbd.utils +.. automodule:: gym_gz.rbd.utils :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.rst index 7feeb9666..285a3ad21 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.rst @@ -1,7 +1,7 @@ -gym\_ignition +gym\_gz ============= -.. automodule:: gym_ignition +.. automodule:: gym_gz :members: :undoc-members: :show-inheritance: @@ -10,10 +10,10 @@ gym\_ignition .. toctree:: :maxdepth: 4 - gym_ignition.base - gym_ignition.context - gym_ignition.randomizers - gym_ignition.rbd - gym_ignition.runtimes - gym_ignition.scenario - gym_ignition.utils + gym_gz.base + gym_gz.context + gym_gz.randomizers + gym_gz.rbd + gym_gz.runtimes + gym_gz.scenario + gym_gz.utils diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.runtimes.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.runtimes.rst index f9eae122c..5060bd38e 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.runtimes.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.runtimes.rst @@ -1,24 +1,24 @@ -gym\_ignition.runtimes +gym\_gz.runtimes ====================== -.. automodule:: gym_ignition.runtimes +.. automodule:: gym_gz.runtimes :members: :undoc-members: :show-inheritance: -gym\_ignition.runtimes.gazebo\_runtime +gym\_gz.runtimes.gazebo\_runtime -------------------------------------- -.. automodule:: gym_ignition.runtimes.gazebo_runtime +.. automodule:: gym_gz.runtimes.gazebo_runtime :members: :undoc-members: :show-inheritance: -gym\_ignition.runtimes.realtime\_runtime +gym\_gz.runtimes.realtime\_runtime ---------------------------------------- -.. automodule:: gym_ignition.runtimes.realtime_runtime +.. automodule:: gym_gz.runtimes.realtime_runtime :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.scenario.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.scenario.rst index 68cedba06..873ddc34f 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.scenario.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.scenario.rst @@ -1,24 +1,24 @@ -gym\_ignition.scenario +gym\_gz.scenario ====================== -.. automodule:: gym_ignition.scenario +.. automodule:: gym_gz.scenario :members: :undoc-members: :show-inheritance: -gym\_ignition.scenario.model\_with\_file +gym\_gz.scenario.model\_with\_file ---------------------------------------- -.. automodule:: gym_ignition.scenario.model_with_file +.. automodule:: gym_gz.scenario.model_with_file :members: :undoc-members: :show-inheritance: -gym\_ignition.scenario.model\_wrapper +gym\_gz.scenario.model\_wrapper ------------------------------------- -.. automodule:: gym_ignition.scenario.model_wrapper +.. automodule:: gym_gz.scenario.model_wrapper :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/apidoc/gym-ignition/gym_ignition.utils.rst b/docs/sphinx/apidoc/gym-ignition/gym_ignition.utils.rst index 8268c4235..2d7f1d25c 100644 --- a/docs/sphinx/apidoc/gym-ignition/gym_ignition.utils.rst +++ b/docs/sphinx/apidoc/gym-ignition/gym_ignition.utils.rst @@ -1,56 +1,56 @@ -gym\_ignition.utils +gym\_gz.utils =================== -.. automodule:: gym_ignition.utils +.. automodule:: gym_gz.utils :members: :undoc-members: :show-inheritance: -gym\_ignition.utils.logger +gym\_gz.utils.logger -------------------------- -.. automodule:: gym_ignition.utils.logger +.. automodule:: gym_gz.utils.logger :members: :undoc-members: :show-inheritance: -gym\_ignition.utils.math +gym\_gz.utils.math ------------------------ -.. automodule:: gym_ignition.utils.math +.. automodule:: gym_gz.utils.math :members: :undoc-members: :show-inheritance: -gym\_ignition.utils.misc +gym\_gz.utils.misc ------------------------ -.. automodule:: gym_ignition.utils.misc +.. automodule:: gym_gz.utils.misc :members: :undoc-members: :show-inheritance: -gym\_ignition.utils.resource\_finder +gym\_gz.utils.resource\_finder ------------------------------------ -.. automodule:: gym_ignition.utils.resource_finder +.. automodule:: gym_gz.utils.resource_finder :members: :undoc-members: :show-inheritance: -gym\_ignition.utils.scenario +gym\_gz.utils.scenario ---------------------------- -.. automodule:: gym_ignition.utils.scenario +.. automodule:: gym_gz.utils.scenario :members: :undoc-members: :show-inheritance: -gym\_ignition.utils.typing +gym\_gz.utils.typing -------------------------- -.. automodule:: gym_ignition.utils.typing +.. automodule:: gym_gz.utils.typing :members: :undoc-members: :show-inheritance: diff --git a/docs/sphinx/conf.py b/docs/sphinx/conf.py index f3d87055b..5874bf254 100644 --- a/docs/sphinx/conf.py +++ b/docs/sphinx/conf.py @@ -17,7 +17,7 @@ # -- Project information ----------------------------------------------------- from datetime import datetime -project = "gym-ignition" +project = "gym-gz" copyright = f"{datetime.now().year}, Istituto Italiano di Tecnologia" author = "Diego Ferigo" diff --git a/docs/sphinx/getting_started/scenario.rst b/docs/sphinx/getting_started/scenario.rst index b4bb48c3f..e709081ad 100644 --- a/docs/sphinx/getting_started/scenario.rst +++ b/docs/sphinx/getting_started/scenario.rst @@ -169,4 +169,4 @@ C++ .. note:: The environment should be properly configured to find the plugins and the models. - Use ``IGN_GAZEBO_SYSTEM_PLUGIN_PATH`` for the plugins and ``IGN_GAZEBO_RESOURCE_PATH`` for the models and meshes. + Use ``GZ_SIM_SYSTEM_PLUGIN_PATH`` for the plugins and ``GZ_SIM_RESOURCE_PATH`` for the models and meshes. diff --git a/docs/sphinx/index.rst b/docs/sphinx/index.rst index 3bb6bf52b..54df44d6a 100644 --- a/docs/sphinx/index.rst +++ b/docs/sphinx/index.rst @@ -1,6 +1,6 @@ -.. _scenario_and_gym_ignition: +.. _scenario_and_gym_gz: -ScenarIO and gym-ignition +ScenarIO and gym-gz ========================= This project targets both *control* and *robot learning* research domains: @@ -12,11 +12,11 @@ This project targets both *control* and *robot learning* research domains: We provide two related subprojects to each of these categories: 1. **ScenarIO** provides APIs to interface with the robots. -2. **gym-ignition** helps structuring environments compatible with OpenAI Gym, +2. **gym-gz** helps structuring environments compatible with OpenAI Gym, while minimizing boilerplate code and providing common rigid-body dynamics utilities. Check the sections :ref:`What is ScenarIO ` and -:ref:`What is gym-ignition ` for more details, +:ref:`What is gym-gz ` for more details, and visit :ref:`Motivations ` for an extended overview. For a quick practical introduction, visit the :ref:`Getting Started ` page. @@ -39,7 +39,7 @@ If you use this project for your research, please check the FAQ about :ref:`how :caption: What what/what_is_scenario - what/what_is_gym_ignition + what/what_is_gym_gz .. toctree:: :hidden: @@ -48,8 +48,8 @@ If you use this project for your research, please check the FAQ about :ref:`how why/motivations why/why_scenario - why/why_ignition_gazebo - why/why_gym_ignition + why/why_gz_sim + why/why_gym_gz .. toctree:: :hidden: @@ -68,7 +68,7 @@ If you use this project for your research, please check the FAQ about :ref:`how getting_started/scenario getting_started/manipulation - getting_started/gym-ignition + getting_started/gym-gz .. toctree:: :hidden: @@ -84,8 +84,8 @@ If you use this project for your research, please check the FAQ about :ref:`how :caption: Python Packages apidoc/scenario/scenario.bindings - apidoc/gym-ignition/gym_ignition - apidoc/gym-ignition-environments/gym_ignition_environments + apidoc/gym-gz/gym_gz + apidoc/gym-gz-environments/gym_gz_environments .. toctree:: :hidden: diff --git a/docs/sphinx/installation/system_configuration.rst b/docs/sphinx/installation/system_configuration.rst index ae52b9bac..18b022fb2 100644 --- a/docs/sphinx/installation/system_configuration.rst +++ b/docs/sphinx/installation/system_configuration.rst @@ -11,6 +11,6 @@ After you enabled the workspace by sourcing its bash script, you may need to als .. code-block:: bash - export IGN_GAZEBO_PHYSICS_ENGINE_PATH=${IGN_GAZEBO_PHYSICS_ENGINE_PATH}:${COLCON_PREFIX_PATH}/lib/ign-physics-3/engine-plugins/ + export GZ_SIM_PHYSICS_ENGINE_PATH=${GZ_SIM_PHYSICS_ENGINE_PATH}:${COLCON_PREFIX_PATH}/lib/ign-physics-3/engine-plugins/ Make sure to select the folder corresponding to the correct version of ign-physics, otherwise the wrong plugins are found. diff --git a/examples/panda_pick_and_place.py b/examples/panda_pick_and_place.py index 841099c2f..5b6c3d30e 100644 --- a/examples/panda_pick_and_place.py +++ b/examples/panda_pick_and_place.py @@ -3,12 +3,12 @@ from functools import partial from typing import List -import gym_ignition -import gym_ignition_environments +import gym_gz +import gym_gz_environments import numpy as np -from gym_ignition.context.gazebo import controllers -from gym_ignition.rbd import conversions -from gym_ignition.rbd.idyntree import inverse_kinematics_nlp +from gym_gz.context.gazebo import controllers +from gym_gz.rbd import conversions +from gym_gz.rbd.idyntree import inverse_kinematics_nlp from scipy.spatial.transform import Rotation as R from scenario import core as scenario_core @@ -22,7 +22,7 @@ def add_panda_controller( - panda: gym_ignition_environments.models.panda.Panda, controller_period: float + panda: gym_gz_environments.models.panda.Panda, controller_period: float ) -> None: # Set the controller period @@ -54,7 +54,7 @@ def add_panda_controller( def get_panda_ik( - panda: gym_ignition_environments.models.panda.Panda, optimized_joints: List[str] + panda: gym_gz_environments.models.panda.Panda, optimized_joints: List[str] ) -> inverse_kinematics_nlp.InverseKinematicsNLP: # Create IK @@ -94,7 +94,7 @@ def get_panda_ik( def insert_bucket(world: scenario_gazebo.World) -> scenario_gazebo.Model: # Insert objects from Fuel - uri = lambda org, name: f"https://fuel.ignitionrobotics.org/{org}/models/{name}" + uri = lambda org, name: f"https://fuel.gazebosim.org/{org}/models/{name}" # Download the cube SDF file bucket_sdf = scenario_gazebo.get_model_file_from_fuel( @@ -120,7 +120,7 @@ def insert_bucket(world: scenario_gazebo.World) -> scenario_gazebo.Model: def insert_table(world: scenario_gazebo.World) -> scenario_gazebo.Model: # Insert objects from Fuel - uri = lambda org, name: f"https://fuel.ignitionrobotics.org/{org}/models/{name}" + uri = lambda org, name: f"https://fuel.gazebosim.org/{org}/models/{name}" # Download the cube SDF file bucket_sdf = scenario_gazebo.get_model_file_from_fuel( @@ -142,7 +142,7 @@ def insert_cube_in_operating_area( ) -> scenario_gazebo.Model: # Insert objects from Fuel - uri = lambda org, name: f"https://fuel.ignitionrobotics.org/{org}/models/{name}" + uri = lambda org, name: f"https://fuel.gazebosim.org/{org}/models/{name}" # Download the cube SDF file cube_sdf = scenario_gazebo.get_model_file_from_fuel( @@ -153,7 +153,7 @@ def insert_cube_in_operating_area( random_position = np.random.uniform(low=[0.2, -0.3, 1.01], high=[0.4, 0.3, 1.01]) # Get a unique name - model_name = gym_ignition.utils.scenario.get_unique_model_name( + model_name = gym_gz.utils.scenario.get_unique_model_name( world=world, model_name="cube" ) @@ -215,7 +215,7 @@ class FingersAction(enum.Enum): def move_fingers( - panda: gym_ignition_environments.models.panda.Panda, action: FingersAction + panda: gym_gz_environments.models.panda.Panda, action: FingersAction ) -> None: # Get the joints of the fingers @@ -236,7 +236,7 @@ def move_fingers( # ==================== # Get the simulator and the world -gazebo, world = gym_ignition.utils.scenario.init_gazebo_sim( +gazebo, world = gym_gz.utils.scenario.init_gazebo_sim( step_size=0.001, real_time_factor=2.0, steps_per_run=1 ) @@ -246,7 +246,7 @@ def move_fingers( gazebo.run(paused=True) # Insert the Panda manipulator -panda = gym_ignition_environments.models.panda.Panda( +panda = gym_gz_environments.models.panda.Panda( world=world, position=[-0.1, 0, 1.0] ) diff --git a/examples/python/launch_cartpole.py b/examples/python/launch_cartpole.py index 9ca935882..292b97438 100755 --- a/examples/python/launch_cartpole.py +++ b/examples/python/launch_cartpole.py @@ -5,9 +5,9 @@ import functools import time -import gym -from gym_ignition.utils import logger -from gym_ignition_environments import randomizers +import gymnasium as gym +from gym_gz.utils import logger +from gym_gz_environments import randomizers # Set verbosity logger.set_level(gym.logger.ERROR) @@ -20,8 +20,8 @@ def make_env_from_id(env_id: str, **kwargs) -> gym.Env: - import gym - import gym_ignition_environments + import gymnasium as gym + import gym_gz_environments return gym.make(env_id, **kwargs) @@ -41,13 +41,11 @@ def make_env_from_id(env_id: str, **kwargs) -> gym.Env: # Enable the rendering # env.render('human') -# Initialize the seed -env.seed(42) - for epoch in range(10): # Reset the environment - observation = env.reset() + # Initialize the seed + observation = env.reset(seed=42, options={}) # Initialize returned values done = False diff --git a/python/gym_ignition/__init__.py b/python/gym_gz/__init__.py similarity index 65% rename from python/gym_ignition/__init__.py rename to python/gym_gz/__init__.py index 8aca5b06a..8574e1da2 100644 --- a/python/gym_ignition/__init__.py +++ b/python/gym_gz/__init__.py @@ -4,33 +4,33 @@ # Workaround for https://github.com/osrf/sdformat/issues/227. # It has to be done before loading the bindings. -import gym_ignition_models +import gym_gz_models -gym_ignition_models.setup_environment() +gym_gz_models.setup_environment() -# Add IGN_GAZEBO_RESOURCE_PATH to the default search path +# Add GZ_SIM_RESOURCE_PATH to the default search path import os -from gym_ignition.utils import resource_finder +from gym_gz.utils import resource_finder -if "IGN_GAZEBO_RESOURCE_PATH" in os.environ: - resource_finder.add_path_from_env_var("IGN_GAZEBO_RESOURCE_PATH") +if "GZ_SIM_RESOURCE_PATH" in os.environ: + resource_finder.add_path_from_env_var("GZ_SIM_RESOURCE_PATH") def initialize_verbosity() -> None: - import gym - import gym_ignition.utils.logger + import gymnasium as gym + import gym_gz.utils.logger import scenario if scenario.detect_install_mode() is scenario.InstallMode.Developer: - gym_ignition.utils.logger.set_level( + gym_gz.utils.logger.set_level( level=gym.logger.INFO, scenario_level=gym.logger.WARN ) elif scenario.detect_install_mode() is scenario.InstallMode.User: - gym_ignition.utils.logger.set_level( + gym_gz.utils.logger.set_level( level=gym.logger.WARN, scenario_level=gym.logger.WARN ) diff --git a/python/gym_ignition/base/__init__.py b/python/gym_gz/base/__init__.py similarity index 100% rename from python/gym_ignition/base/__init__.py rename to python/gym_gz/base/__init__.py diff --git a/python/gym_ignition/base/runtime.py b/python/gym_gz/base/runtime.py similarity index 72% rename from python/gym_ignition/base/runtime.py rename to python/gym_gz/base/runtime.py index 1cc1fd72a..96813ee8d 100644 --- a/python/gym_ignition/base/runtime.py +++ b/python/gym_gz/base/runtime.py @@ -4,31 +4,31 @@ import abc -import gym -from gym_ignition.base.task import Task +import gymnasium as gym +from gym_gz.base.task import Task class Runtime(gym.Env, abc.ABC): """ - Base class for defining executors of :py:class:`~gym_ignition.base.task.Task` objects. + Base class for defining executors of :py:class:`~gym_gz.base.task.Task` objects. - :py:class:`~gym_ignition.base.task.Task` classes are supposed to be generic and are + :py:class:`~gym_gz.base.task.Task` classes are supposed to be generic and are not tied to any specific runtime. Implementations of a runtime class contain all the logic to define how to execute the task, allowing to run the same - :py:class:`~gym_ignition.base.task.Task` class on different simulators or in a + :py:class:`~gym_gz.base.task.Task` class on different simulators or in a real-time setting. Runtimes are the real :py:class:`gym.Env` objects returned to the users when they call the :py:class:`gym.make` factory method. Args: - task: the :py:class:`~gym_ignition.base.task.Task` object to handle. + task: the :py:class:`~gym_gz.base.task.Task` object to handle. agent_rate: the rate at which the environment will be called. Sometimes tasks need to know this information. Example: Here is minimal example of how the :py:class:`Runtime`, :py:class:`gym.Env` and - :py:class:`~gym_ignition.base.task.Task` could be integrated: + :py:class:`~gym_gz.base.task.Task` could be integrated: .. code-block:: python @@ -38,26 +38,27 @@ def __init__(self, task): super().__init__(task=task, agent_rate=agent_rate) self.action_space, self.observation_space = self.task.create_spaces() - def reset(self): + def reset(self, seed=None, options={}, **kwargs): self.task.reset_task() - return self.task.get_observation() + return self.task.get_observation(), self.task.get_info() def step(self, action): self.task.set_action(action) # [...] code that performs the real step [...] - done = self.task.is_done() + terminated = self.task.is_terminated() + truncated = self.task.is_truncated() reward = self.task.get_reward() observation = self.task.get_observation() - return observation, reward, done, {} + return observation, reward, terminated, truncated, {} def close(self): pass Note: - Runtimes can handle only one :py:class:`~gym_ignition.base.task.Task` object. + Runtimes can handle only one :py:class:`~gym_gz.base.task.Task` object. """ def __init__(self, task: Task, agent_rate: float): diff --git a/python/gym_ignition/base/task.py b/python/gym_gz/base/task.py similarity index 87% rename from python/gym_ignition/base/task.py rename to python/gym_gz/base/task.py index 40aaf2a02..037d7cf6c 100644 --- a/python/gym_ignition/base/task.py +++ b/python/gym_gz/base/task.py @@ -5,10 +5,10 @@ import abc from typing import Dict, Tuple -import gym +import gymnasium as gym import numpy as np -from gym.utils import seeding -from gym_ignition.utils.typing import ( +from gymnasium.utils import seeding +from gym_gz.utils.typing import ( Action, ActionSpace, Observation, @@ -28,10 +28,10 @@ class Task(abc.ABC): It defines the logic of the environment in a format that is agnostic of both the runtime (either simulated or real-time) and the models it operates on. - :py:class:`~gym_ignition.base.runtime.Runtime` instances are the real objects returned + :py:class:`~gym_gz.base.runtime.Runtime` instances are the real objects returned to the users when they call :py:class:`gym.make`. Depending on the type of the runtime, it could contain one or more :py:class:`Task` objects. - The :py:class:`~gym_ignition.base.runtime.Runtime` is a relay class that calls the + The :py:class:`~gym_gz.base.runtime.Runtime` is a relay class that calls the logic of the :py:class:`Task` from its interface methods and implements the real :py:meth:`gym.Env.step`. Simulated runtimes step the physics engine, instead, real-time @@ -39,7 +39,7 @@ class Task(abc.ABC): A :py:class:`Task` object is meant to be: - - Independent from the selected :py:class:`~gym_ignition.base.runtime.Runtime`. + - Independent from the selected :py:class:`~gym_gz.base.runtime.Runtime`. In fact, it defines only the decision making logic; - Independent from the :py:class:`~scenario.core.Model` objects it operates on. This is achieved thanks to the model abstraction provided by @@ -192,7 +192,7 @@ def get_reward(self) -> Reward: """ @abc.abstractmethod - def is_done(self) -> bool: + def is_terminated(self) -> bool: """ Return the task termination flag. @@ -208,6 +208,24 @@ def is_done(self) -> bool: Returns: True if the environment terminated, False otherwise. """ + + @abc.abstractmethod + def is_truncated(self) -> bool: + """ + Return the task truncation flag. + + This method contains the logic for defining when the environment has truncated. + Subsequent calls to :py:meth:`Task.set_action` should be preceded by a task + reset through :py:meth:`Task.reset_task`. + + It is called in the end of the :py:meth:`gym.Env.step` method. + + Raises: + RuntimeError: In case of failure. + + Returns: + True if the environment truncated, False otherwise. + """ def get_info(self) -> Dict: """ @@ -234,7 +252,7 @@ def seed_task(self, seed: int = None) -> SeedList: # Create the seed if not passed seed = np.random.randint(2 ** 32 - 1) if seed is None else seed - # Get an instance of the random number generator from gym utils. + # Get an instance of the random number generator from gymnasium utils. # This is necessary to have an independent rng for each environment. self.np_random, self.seed = seeding.np_random(seed) diff --git a/python/gym_ignition/context/__init__.py b/python/gym_gz/context/__init__.py similarity index 100% rename from python/gym_ignition/context/__init__.py rename to python/gym_gz/context/__init__.py diff --git a/python/gym_ignition/context/gazebo/__init__.py b/python/gym_gz/context/gazebo/__init__.py similarity index 100% rename from python/gym_ignition/context/gazebo/__init__.py rename to python/gym_gz/context/gazebo/__init__.py diff --git a/python/gym_ignition/context/gazebo/controllers.py b/python/gym_gz/context/gazebo/controllers.py similarity index 96% rename from python/gym_ignition/context/gazebo/controllers.py rename to python/gym_gz/context/gazebo/controllers.py index b746130d5..34b52a037 100644 --- a/python/gym_ignition/context/gazebo/controllers.py +++ b/python/gym_gz/context/gazebo/controllers.py @@ -5,7 +5,7 @@ from dataclasses import dataclass, field from typing import Iterable, List, Tuple -from gym_ignition.context.gazebo import plugin +from gym_gz.context.gazebo import plugin GRAVITY = (0, 0, -9.80665) diff --git a/python/gym_ignition/context/gazebo/plugin.py b/python/gym_gz/context/gazebo/plugin.py similarity index 93% rename from python/gym_ignition/context/gazebo/plugin.py rename to python/gym_gz/context/gazebo/plugin.py index 5bf8a73fa..0c83ba274 100644 --- a/python/gym_ignition/context/gazebo/plugin.py +++ b/python/gym_gz/context/gazebo/plugin.py @@ -16,10 +16,10 @@ @dataclass class GazeboPlugin(abc.ABC): """ - Base class of all World and Model plugins for Ignition Gazebo. + Base class of all World and Model plugins for Gz sim. The Plugin abstract class provides boilerplate code that simplifies and streamlines - the definition of helper classes that insert Ignition Gazebo plugins to either World + the definition of helper classes that insert Gz sim plugins to either World or Model objects. Classes that inherit from Plugin have to provide the following information: diff --git a/python/gym_ignition/randomizers/__init__.py b/python/gym_gz/randomizers/__init__.py similarity index 100% rename from python/gym_ignition/randomizers/__init__.py rename to python/gym_gz/randomizers/__init__.py diff --git a/python/gym_ignition/randomizers/abc.py b/python/gym_gz/randomizers/abc.py similarity index 85% rename from python/gym_ignition/randomizers/abc.py rename to python/gym_gz/randomizers/abc.py index e0f61eb74..641c5a4a2 100644 --- a/python/gym_ignition/randomizers/abc.py +++ b/python/gym_gz/randomizers/abc.py @@ -4,22 +4,22 @@ import abc -import gym_ignition.base.task +import gym_gz.base.task from scenario import core as scenario_core class TaskRandomizer(abc.ABC): @abc.abstractmethod - def randomize_task(self, task: gym_ignition.base.task.Task, **kwargs) -> None: + def randomize_task(self, task: gym_gz.base.task.Task, **kwargs) -> None: """ - Randomize a :py:class:`~gym_ignition.base.task.Task` instance. + Randomize a :py:class:`~gym_gz.base.task.Task` instance. Args: task: the task to randomize. Note: - Note that each task has a :py:attr:`~gym_ignition.base.task.Task.world` + Note that each task has a :py:attr:`~gym_gz.base.task.Task.world` property that provides access to the simulated :py:class:`scenario.bindings.core.World`. """ @@ -41,14 +41,14 @@ def __init__(self, randomize_after_rollouts_num: int = 0): self.randomize_after_rollouts_num = randomize_after_rollouts_num @abc.abstractmethod - def randomize_physics(self, task: gym_ignition.base.task.Task, **kwargs) -> None: + def randomize_physics(self, task: gym_gz.base.task.Task, **kwargs) -> None: """ Method that insert and configures the physics of a Task's world. By default this method loads a plugin that uses DART with no randomizations. Randomizing physics engine parameters or changing physics engine backend could be done by redefining this method and passing it to - :py:class:`~gym_ignition.runtimes.gazebo_runtime.GazeboRuntime`. + :py:class:`~gym_gz.runtimes.gazebo_runtime.GazeboRuntime`. Args: task: A task containing a world object without physics. @@ -102,7 +102,7 @@ def physics_expired(self) -> bool: class ModelRandomizer(abc.ABC): @abc.abstractmethod def randomize_model( - self, task: gym_ignition.base.task.Task, **kwargs + self, task: gym_gz.base.task.Task, **kwargs ) -> scenario_core.Model: """ Randomize the model. @@ -119,7 +119,7 @@ def randomize_model( class ModelDescriptionRandomizer(abc.ABC): @abc.abstractmethod def randomize_model_description( - self, task: gym_ignition.base.task.Task, **kwargs + self, task: gym_gz.base.task.Task, **kwargs ) -> str: """ Randomize the model description. diff --git a/python/gym_ignition/randomizers/gazebo_env_randomizer.py b/python/gym_gz/randomizers/gazebo_env_randomizer.py similarity index 79% rename from python/gym_ignition/randomizers/gazebo_env_randomizer.py rename to python/gym_gz/randomizers/gazebo_env_randomizer.py index f1690231b..c21ab29d3 100644 --- a/python/gym_ignition/randomizers/gazebo_env_randomizer.py +++ b/python/gym_gz/randomizers/gazebo_env_randomizer.py @@ -5,22 +5,22 @@ import abc from typing import Callable, Dict, Optional, Union, cast -import gym -from gym_ignition.randomizers.abc import PhysicsRandomizer, TaskRandomizer -from gym_ignition.randomizers.physics import dart -from gym_ignition.runtimes import gazebo_runtime -from gym_ignition.utils import typing +import gymnasium as gym +from gym_gz.randomizers.abc import PhysicsRandomizer, TaskRandomizer +from gym_gz.randomizers.physics import dart +from gym_gz.runtimes import gazebo_runtime +from gym_gz.utils import typing MakeEnvCallable = Callable[[Optional[Dict]], gym.Env] class GazeboEnvRandomizer(gym.Wrapper, TaskRandomizer, abc.ABC): """ - Base class to implement an environment randomizer for Ignition Gazebo. + Base class to implement an environment randomizer for Gz sim. The randomizer is a :py:class:`gym.Wrapper` that extends the :py:meth:`gym.Env.reset` method. Objects that inherit from this class are used to - setup the environment for the handled :py:class:`~gym_ignition.base.task.Task`. + setup the environment for the handled :py:class:`~gym_gz.base.task.Task`. In its simplest form, a randomizer populates the world with all the models that need to be part of the simulation. The task could then operate on them from a @@ -28,8 +28,8 @@ class GazeboEnvRandomizer(gym.Wrapper, TaskRandomizer, abc.ABC): More complex environments may require to randomize one or more simulated entities. Concrete classes that implement a randomizer could use - :py:class:`~gym_ignition.randomizers.model.sdf.SDFRandomizer` to randomize the model - and :py:class:`~gym_ignition.randomizers.abc.PhysicsRandomizer` to randomize + :py:class:`~gym_gz.randomizers.model.sdf.SDFRandomizer` to randomize the model + and :py:class:`~gym_gz.randomizers.abc.PhysicsRandomizer` to randomize the physics. Args: @@ -72,13 +72,13 @@ def __init__( # gym.Env methods # =============== - def reset(self, **kwargs) -> typing.Observation: + def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> typing.ResetReturn: # Reset the physics - if self._physics_randomizer.physics_expired(): + if self._physics_randomizer.physics_expired() and seed is None: # Get the random components of the task - seed = self.env.task.seed + seed = self.env.unwrapped.task.seed np_random = self.env.task.np_random # Reset the runtime + task, creating a new Gazebo instance @@ -86,24 +86,23 @@ def reset(self, **kwargs) -> typing.Observation: del self.env self.env = self._create_environment(self._env_option, **self._kwargs) - # Restore the random components - self.env.seed(seed=seed) - assert self.env.task.seed == seed + # Restore the random components of the task + assert self.env.unwrapped.task.seed == seed self.env.task.np_random = np_random # Mark the beginning of a new rollout self._physics_randomizer.increase_rollout_counter() # Reset the task through the TaskRandomizer - self.randomize_task(task=self.env.task, gazebo=self.env.gazebo, **kwargs) + self.randomize_task(task=self.env.unwrapped.task, gazebo=self.env.unwrapped.gazebo, **kwargs) - ok_paused_run = self.env.gazebo.run(paused=True) + ok_paused_run = self.env.unwrapped.gazebo.run(paused=True) if not ok_paused_run: raise RuntimeError("Failed to execute a paused Gazebo run") # Reset the Task - return self.env.reset() + return self.env.reset(seed=seed, options={}) # =============== # Private methods diff --git a/python/gym_ignition/randomizers/model/__init__.py b/python/gym_gz/randomizers/model/__init__.py similarity index 100% rename from python/gym_ignition/randomizers/model/__init__.py rename to python/gym_gz/randomizers/model/__init__.py diff --git a/python/gym_ignition/randomizers/model/sdf.py b/python/gym_gz/randomizers/model/sdf.py similarity index 97% rename from python/gym_ignition/randomizers/model/sdf.py rename to python/gym_gz/randomizers/model/sdf.py index 52b274b4f..6c5374342 100644 --- a/python/gym_ignition/randomizers/model/sdf.py +++ b/python/gym_gz/randomizers/model/sdf.py @@ -46,11 +46,11 @@ class UniformParams(NamedTuple): class RandomizationDataBuilder: """ - Builder class of a :py:class:`~gym_ignition.randomizers.model.sdf.RandomizationData` + Builder class of a :py:class:`~gym_gz.randomizers.model.sdf.RandomizationData` object. Args: - randomizer: The :py:class:`~gym_ignition.randomizers.model.sdf.SDFRandomizer` + randomizer: The :py:class:`~gym_gz.randomizers.model.sdf.SDFRandomizer` object to which the created randomization will be inserted. """ @@ -211,7 +211,7 @@ def find_xpath(self, xpath: str) -> List[etree.Element]: Find the elements that match an XPath pattern. This method could be helpful to test the matches of a XPath pattern before using - it in :py:meth:`~gym_ignition.randomizers.model.sdf.RandomizationDataBuilder.at_xpath`. + it in :py:meth:`~gym_gz.randomizers.model.sdf.RandomizationDataBuilder.at_xpath`. Args: xpath: The XPath pattern. diff --git a/python/gym_ignition/randomizers/physics/__init__.py b/python/gym_gz/randomizers/physics/__init__.py similarity index 100% rename from python/gym_ignition/randomizers/physics/__init__.py rename to python/gym_gz/randomizers/physics/__init__.py diff --git a/python/gym_ignition/randomizers/physics/dart.py b/python/gym_gz/randomizers/physics/dart.py similarity index 80% rename from python/gym_ignition/randomizers/physics/dart.py rename to python/gym_gz/randomizers/physics/dart.py index 59f25aae2..539b2a206 100644 --- a/python/gym_ignition/randomizers/physics/dart.py +++ b/python/gym_gz/randomizers/physics/dart.py @@ -2,8 +2,8 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -import gym_ignition.base.task -from gym_ignition import randomizers +import gym_gz.base.task +from gym_gz import randomizers from scenario import gazebo as scenario @@ -24,6 +24,6 @@ def get_engine(self): return scenario.PhysicsEngine_dart - def randomize_physics(self, task: gym_ignition.base.task.Task, **kwargs) -> None: + def randomize_physics(self, task: gym_gz.base.task.Task, **kwargs) -> None: pass diff --git a/python/gym_ignition/rbd/__init__.py b/python/gym_gz/rbd/__init__.py similarity index 100% rename from python/gym_ignition/rbd/__init__.py rename to python/gym_gz/rbd/__init__.py diff --git a/python/gym_ignition/rbd/conversions.py b/python/gym_gz/rbd/conversions.py similarity index 100% rename from python/gym_ignition/rbd/conversions.py rename to python/gym_gz/rbd/conversions.py diff --git a/python/gym_ignition/rbd/idyntree/__init__.py b/python/gym_gz/rbd/idyntree/__init__.py similarity index 100% rename from python/gym_ignition/rbd/idyntree/__init__.py rename to python/gym_gz/rbd/idyntree/__init__.py diff --git a/python/gym_ignition/rbd/idyntree/helpers.py b/python/gym_gz/rbd/idyntree/helpers.py similarity index 98% rename from python/gym_ignition/rbd/idyntree/helpers.py rename to python/gym_gz/rbd/idyntree/helpers.py index 58e1c1c25..f1a83ec3e 100644 --- a/python/gym_ignition/rbd/idyntree/helpers.py +++ b/python/gym_gz/rbd/idyntree/helpers.py @@ -8,7 +8,7 @@ from typing import List import idyntree.bindings as idt -from gym_ignition.utils import resource_finder +from gym_gz.utils import resource_finder class FrameVelocityRepresentation(Enum): diff --git a/python/gym_ignition/rbd/idyntree/inverse_kinematics_nlp.py b/python/gym_gz/rbd/idyntree/inverse_kinematics_nlp.py similarity index 99% rename from python/gym_ignition/rbd/idyntree/inverse_kinematics_nlp.py rename to python/gym_gz/rbd/idyntree/inverse_kinematics_nlp.py index 13d90edc5..18edbada5 100644 --- a/python/gym_ignition/rbd/idyntree/inverse_kinematics_nlp.py +++ b/python/gym_gz/rbd/idyntree/inverse_kinematics_nlp.py @@ -9,7 +9,7 @@ import idyntree.bindings as idt import numpy as np -from gym_ignition import rbd +from gym_gz import rbd class TargetType(Enum): diff --git a/python/gym_ignition/rbd/idyntree/kindyncomputations.py b/python/gym_gz/rbd/idyntree/kindyncomputations.py similarity index 99% rename from python/gym_ignition/rbd/idyntree/kindyncomputations.py rename to python/gym_gz/rbd/idyntree/kindyncomputations.py index a20a0d1e2..3c3b5fb01 100644 --- a/python/gym_ignition/rbd/idyntree/kindyncomputations.py +++ b/python/gym_gz/rbd/idyntree/kindyncomputations.py @@ -6,8 +6,8 @@ import idyntree.bindings as idt import numpy as np -from gym_ignition.rbd import conversions -from gym_ignition.rbd.idyntree import numpy +from gym_gz.rbd import conversions +from gym_gz.rbd.idyntree import numpy from scenario import core as scenario_core diff --git a/python/gym_ignition/rbd/idyntree/numpy.py b/python/gym_gz/rbd/idyntree/numpy.py similarity index 99% rename from python/gym_ignition/rbd/idyntree/numpy.py rename to python/gym_gz/rbd/idyntree/numpy.py index 8a7268997..ec66a1cad 100644 --- a/python/gym_ignition/rbd/idyntree/numpy.py +++ b/python/gym_gz/rbd/idyntree/numpy.py @@ -3,7 +3,7 @@ import idyntree.bindings as idt import numpy as np -from gym_ignition import rbd +from gym_gz import rbd class FromNumPy(abc.ABC): diff --git a/python/gym_ignition/rbd/utils.py b/python/gym_gz/rbd/utils.py similarity index 100% rename from python/gym_ignition/rbd/utils.py rename to python/gym_gz/rbd/utils.py diff --git a/python/gym_ignition/runtimes/__init__.py b/python/gym_gz/runtimes/__init__.py similarity index 100% rename from python/gym_ignition/runtimes/__init__.py rename to python/gym_gz/runtimes/__init__.py diff --git a/python/gym_ignition/runtimes/gazebo_runtime.py b/python/gym_gz/runtimes/gazebo_runtime.py similarity index 89% rename from python/gym_ignition/runtimes/gazebo_runtime.py rename to python/gym_gz/runtimes/gazebo_runtime.py index 25335876b..ca04fcf10 100644 --- a/python/gym_ignition/runtimes/gazebo_runtime.py +++ b/python/gym_gz/runtimes/gazebo_runtime.py @@ -2,21 +2,24 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -from typing import Optional +from typing import Optional, Dict -import gym_ignition_models -from gym_ignition import base -from gym_ignition.base import runtime -from gym_ignition.utils import logger -from gym_ignition.utils.typing import * +import gymnasium as gym +import numpy as np + +import gym_gz_models +from gym_gz import base +from gym_gz.base import runtime +from gym_gz.utils import logger +from gym_gz.utils.typing import Action, Info, Observation, Reward, SeedList, State, Terminated, Truncated, ResetReturn from scenario import gazebo as scenario class GazeboRuntime(runtime.Runtime): """ - Implementation of :py:class:`~gym_ignition.base.runtime.Runtime` for the Ignition - Gazebo simulator. + Implementation of :py:class:`~gym_gz.base.runtime.Runtime` for the Gz + Sim simulator. Args: task_cls: The class of the handled task. @@ -33,7 +36,7 @@ class GazeboRuntime(runtime.Runtime): physics, a new simulator should be created. """ - metadata = {"render.modes": ["human"]} + metadata = {"render_modes": ["human"]} def __init__( self, @@ -46,7 +49,7 @@ def __init__( **kwargs, ): - if gym.logger.MIN_LEVEL <= gym.logger.DEBUG: + if gym.logger.min_level <= gym.logger.DEBUG: import inspect frame = inspect.currentframe() @@ -124,14 +127,19 @@ def step(self, action: Action) -> State: assert isinstance(reward, float), "Failed to get the reward" # Check termination - done = self.task.is_done() + terminated = self.task.is_terminated() + + # Check truncation + truncated = self.task.is_truncated() # Get info info = self.task.get_info() - return State((Observation(observation), Reward(reward), Done(done), Info(info))) + return State((Observation(observation), Reward(reward), Terminated(terminated), Truncated(truncated), Info(info))) + + def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> ResetReturn: - def reset(self) -> Observation: + self.seed(seed) # Reset the task self.task.reset_task() @@ -148,8 +156,9 @@ def reset(self) -> Observation: if not self.observation_space.contains(observation): logger.warn("The observation does not belong to the observation space") - - return Observation(observation) + # Get info + info = self.task.get_info() + return ResetReturn((Observation(observation), Info(info))) def render(self, mode: str = "human", **kwargs) -> None: @@ -261,7 +270,7 @@ def world(self) -> scenario.World: # Insert the ground plane ok_ground = world.insert_model( - gym_ignition_models.get_model_file("ground_plane") + gym_gz_models.get_model_file("ground_plane") ) if not ok_ground: diff --git a/python/gym_ignition/runtimes/realtime_runtime.py b/python/gym_gz/runtimes/realtime_runtime.py similarity index 76% rename from python/gym_ignition/runtimes/realtime_runtime.py rename to python/gym_gz/runtimes/realtime_runtime.py index 5a6189ad4..f75fe3431 100644 --- a/python/gym_ignition/runtimes/realtime_runtime.py +++ b/python/gym_gz/runtimes/realtime_runtime.py @@ -2,13 +2,13 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -from gym_ignition.base import runtime, task -from gym_ignition.utils.typing import Action, Done, Info, Observation, State +from gym_gz.base import runtime, task +from gym_gz.utils.typing import Action, Done, Info, Observation, State, Terminated, Truncated, ResetReturn, Dict class RealTimeRuntime(runtime.Runtime): """ - Implementation of :py:class:`~gym_ignition.base.runtime.Runtime` for real-time + Implementation of :py:class:`~gym_gz.base.runtime.Runtime` for real-time execution. Warning: @@ -66,16 +66,19 @@ def step(self, action: Action) -> State: assert reward, "Failed to get the reward" # Check termination - done = self.task.is_done() + terminated = self.task.is_terminated() - return State((observation, reward, Done(done), Info({}))) + #Check truncation + truncated = self.task.is_truncated() - def reset(self) -> Observation: + return State((observation, reward, Terminated(terminated), Truncated(truncated), Info({}))) + + def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> ResetReturn: # Get the observation observation = self.task.get_observation() - return Observation(observation) + return ResetReturn((observation, Info({}))) def render(self, mode: str = "human", **kwargs) -> None: raise NotImplementedError diff --git a/python/gym_ignition/scenario/__init__.py b/python/gym_gz/scenario/__init__.py similarity index 100% rename from python/gym_ignition/scenario/__init__.py rename to python/gym_gz/scenario/__init__.py diff --git a/python/gym_ignition/scenario/model_with_file.py b/python/gym_gz/scenario/model_with_file.py similarity index 100% rename from python/gym_ignition/scenario/model_with_file.py rename to python/gym_gz/scenario/model_with_file.py diff --git a/python/gym_ignition/scenario/model_wrapper.py b/python/gym_gz/scenario/model_wrapper.py similarity index 100% rename from python/gym_ignition/scenario/model_wrapper.py rename to python/gym_gz/scenario/model_wrapper.py diff --git a/python/gym_ignition/utils/__init__.py b/python/gym_gz/utils/__init__.py similarity index 100% rename from python/gym_ignition/utils/__init__.py rename to python/gym_gz/utils/__init__.py diff --git a/python/gym_ignition/utils/logger.py b/python/gym_gz/utils/logger.py similarity index 89% rename from python/gym_ignition/utils/logger.py rename to python/gym_gz/utils/logger.py index ca20a4e18..d352078dd 100644 --- a/python/gym_ignition/utils/logger.py +++ b/python/gym_gz/utils/logger.py @@ -5,10 +5,10 @@ import contextlib import warnings -import gym -from gym import logger -from gym.logger import debug, error, info -from gym.utils import colorize +import gymnasium as gym +from gymnasium import logger +from gymnasium.logger import debug, error, info +from gymnasium.utils import colorize def custom_formatwarning(msg, *args, **kwargs): @@ -16,7 +16,7 @@ def custom_formatwarning(msg, *args, **kwargs): Custom format that overrides :py:func:`warnings.formatwarning`. """ - if logger.MIN_LEVEL is logger.DEBUG: + if logger.min_level is logger.DEBUG: warning = "{}:{} {}: {}\n".format(args[1], args[2], args[0].__name__, msg) else: warning = "{}\n".format(msg) @@ -29,7 +29,7 @@ def warn(msg: str, *args) -> None: Custom definition of :py:func:`gym.logger.warn` function. """ - if logger.MIN_LEVEL <= logger.WARN: + if logger.min_level <= logger.WARN: warnings.warn(colorize("%s: %s" % ("WARN", msg % args), "yellow"), stacklevel=2) @@ -39,7 +39,7 @@ def warn(msg: str, *args) -> None: def set_level(level: int, scenario_level: int = None) -> None: """ - Set the verbosity level of both :py:mod:`gym` and :py:mod:`gym_ignition`. + Set the verbosity level of both :py:mod:`gym` and :py:mod:`gym_gz`. Accepted values: @@ -81,7 +81,7 @@ def set_level(level: int, scenario_level: int = None) -> None: @contextlib.contextmanager def gym_verbosity(level: int): - old_level = gym.logger.MIN_LEVEL + old_level = gym.logger.min_level gym.logger.set_level(level=level) yield None gym.logger.set_level(old_level) diff --git a/python/gym_ignition/utils/math.py b/python/gym_gz/utils/math.py similarity index 100% rename from python/gym_ignition/utils/math.py rename to python/gym_gz/utils/math.py diff --git a/python/gym_ignition/utils/misc.py b/python/gym_gz/utils/misc.py similarity index 100% rename from python/gym_ignition/utils/misc.py rename to python/gym_gz/utils/misc.py diff --git a/python/gym_ignition/utils/resource_finder.py b/python/gym_gz/utils/resource_finder.py similarity index 86% rename from python/gym_ignition/utils/resource_finder.py rename to python/gym_gz/utils/resource_finder.py index 5d507a844..60cfcdb05 100644 --- a/python/gym_ignition/utils/resource_finder.py +++ b/python/gym_gz/utils/resource_finder.py @@ -6,14 +6,14 @@ from os.path import exists, isfile from typing import List -from gym_ignition.utils import logger +from gym_gz.utils import logger -GYM_IGNITION_DATA_PATH = [] +GYM_GZ_DATA_PATH = [] def get_search_paths() -> List[str]: - global GYM_IGNITION_DATA_PATH - return GYM_IGNITION_DATA_PATH + global GYM_GZ_DATA_PATH + return GYM_GZ_DATA_PATH def add_path(data_path: str) -> None: @@ -23,15 +23,15 @@ def add_path(data_path: str) -> None: ) return - global GYM_IGNITION_DATA_PATH + global GYM_GZ_DATA_PATH - for path in GYM_IGNITION_DATA_PATH: + for path in GYM_GZ_DATA_PATH: if path == data_path: logger.debug(f"The path '{data_path}' is already present in the data path") return logger.debug(f"Adding new search path: '{data_path}'") - GYM_IGNITION_DATA_PATH.append(data_path) + GYM_GZ_DATA_PATH.append(data_path) def add_path_from_env_var(env_variable: str) -> None: @@ -55,7 +55,7 @@ def add_path_from_env_var(env_variable: str) -> None: def find_resource(file_name: str) -> str: file_abs_path = "" - global GYM_IGNITION_DATA_PATH + global GYM_GZ_DATA_PATH logger.debug(f"Looking for file '{file_name}'") @@ -68,7 +68,7 @@ def find_resource(file_name: str) -> str: raise FileNotFoundError(f"Failed to find resource '{file_name}'") # Handle if the path is relative - for path in GYM_IGNITION_DATA_PATH: + for path in GYM_GZ_DATA_PATH: logger.debug(f" Exploring folder '{path}'") path_with_slash = path if path[-1] == "/" else path + "/" candidate_abs_path = path_with_slash + file_name diff --git a/python/gym_ignition/utils/scenario.py b/python/gym_gz/utils/scenario.py similarity index 96% rename from python/gym_ignition/utils/scenario.py rename to python/gym_gz/utils/scenario.py index 3c6797762..33884e8a7 100644 --- a/python/gym_ignition/utils/scenario.py +++ b/python/gym_gz/utils/scenario.py @@ -5,7 +5,7 @@ from typing import List, Tuple, Union import gym.spaces -import gym_ignition_models +import gym_gz_models import numpy as np from scenario import core @@ -78,7 +78,7 @@ def init_gazebo_sim( world = gazebo.get_world() # Insert the ground plane - ok_ground = world.insert_model(gym_ignition_models.get_model_file("ground_plane")) + ok_ground = world.insert_model(gym_gz_models.get_model_file("ground_plane")) if not ok_ground: raise RuntimeError("Failed to insert the ground plane") diff --git a/python/gym_ignition/utils/typing.py b/python/gym_gz/utils/typing.py similarity index 74% rename from python/gym_ignition/utils/typing.py rename to python/gym_gz/utils/typing.py index 5b25627da..18f466073 100644 --- a/python/gym_ignition/utils/typing.py +++ b/python/gym_gz/utils/typing.py @@ -7,15 +7,18 @@ import gym.spaces import numpy as np +Terminated = NewType("Terminated", bool) Done = NewType("Done", bool) +Truncated = NewType("Truncated", bool) Info = NewType("Info", Dict) Reward = NewType("Reward", float) Observation = NewType("Observation", np.ndarray) +ResetReturn = NewType("ResetReturn", Tuple[Observation, Info]) Action = NewType("Action", Union[np.ndarray, np.number]) SeedList = NewType("SeedList", List[int]) -State = NewType("State", Tuple[Observation, Reward, Done, Info]) +State = NewType("State", Tuple[Observation, Reward, Terminated, Truncated, Info]) ActionSpace = NewType("ActionSpace", gym.spaces.Space) ObservationSpace = NewType("ObservationSpace", gym.spaces.Space) diff --git a/python/gym_ignition_environments/__init__.py b/python/gym_gz_environments/__init__.py similarity index 78% rename from python/gym_ignition_environments/__init__.py rename to python/gym_gz_environments/__init__.py index 35228f42b..7712ca12b 100644 --- a/python/gym_ignition_environments/__init__.py +++ b/python/gym_gz_environments/__init__.py @@ -3,15 +3,15 @@ # GNU Lesser General Public License v2.1 or any later version. import numpy -from gym.envs.registration import register +from gymnasium.envs.registration import register from . import models, randomizers, tasks -max_float = float(numpy.finfo(numpy.float32).max) +max_float = float(numpy.finfo(numpy.float64).max) register( id="Pendulum-Gazebo-v0", - entry_point="gym_ignition.runtimes.gazebo_runtime:GazeboRuntime", + entry_point="gym_gz.runtimes.gazebo_runtime:GazeboRuntime", max_episode_steps=5000, kwargs={ "task_cls": tasks.pendulum_swingup.PendulumSwingUp, @@ -23,7 +23,7 @@ register( id="CartPoleDiscreteBalancing-Gazebo-v0", - entry_point="gym_ignition.runtimes.gazebo_runtime:GazeboRuntime", + entry_point="gym_gz.runtimes.gazebo_runtime:GazeboRuntime", max_episode_steps=5000, kwargs={ "task_cls": tasks.cartpole_discrete_balancing.CartPoleDiscreteBalancing, @@ -35,7 +35,7 @@ register( id="CartPoleContinuousBalancing-Gazebo-v0", - entry_point="gym_ignition.runtimes.gazebo_runtime:GazeboRuntime", + entry_point="gym_gz.runtimes.gazebo_runtime:GazeboRuntime", max_episode_steps=5000, kwargs={ "task_cls": tasks.cartpole_continuous_balancing.CartPoleContinuousBalancing, @@ -47,7 +47,7 @@ register( id="CartPoleContinuousSwingup-Gazebo-v0", - entry_point="gym_ignition.runtimes.gazebo_runtime:GazeboRuntime", + entry_point="gym_gz.runtimes.gazebo_runtime:GazeboRuntime", max_episode_steps=5000, kwargs={ "task_cls": tasks.cartpole_continuous_swingup.CartPoleContinuousSwingup, diff --git a/python/gym_ignition_environments/models/__init__.py b/python/gym_gz_environments/models/__init__.py similarity index 100% rename from python/gym_ignition_environments/models/__init__.py rename to python/gym_gz_environments/models/__init__.py diff --git a/python/gym_ignition_environments/models/cartpole.py b/python/gym_gz_environments/models/cartpole.py similarity index 85% rename from python/gym_ignition_environments/models/cartpole.py rename to python/gym_gz_environments/models/cartpole.py index 9c1746d4b..14d6cc1a3 100644 --- a/python/gym_ignition_environments/models/cartpole.py +++ b/python/gym_gz_environments/models/cartpole.py @@ -4,8 +4,8 @@ from typing import List -from gym_ignition.scenario import model_with_file, model_wrapper -from gym_ignition.utils.scenario import get_unique_model_name +from gym_gz.scenario import model_with_file, model_wrapper +from gym_gz.utils.scenario import get_unique_model_name from scenario import core as scenario @@ -44,6 +44,6 @@ def __init__( @classmethod def get_model_file(cls) -> str: - import gym_ignition_models + import gym_gz_models - return gym_ignition_models.get_model_file("cartpole") + return gym_gz_models.get_model_file("cartpole") diff --git a/python/gym_ignition_environments/models/icub.py b/python/gym_gz_environments/models/icub.py similarity index 92% rename from python/gym_ignition_environments/models/icub.py rename to python/gym_gz_environments/models/icub.py index b59284fbd..07c024d4c 100644 --- a/python/gym_ignition_environments/models/icub.py +++ b/python/gym_gz_environments/models/icub.py @@ -5,8 +5,8 @@ import abc from typing import List -from gym_ignition import scenario -from gym_ignition.utils.scenario import get_unique_model_name +from gym_gz import scenario +from gym_gz.utils.scenario import get_unique_model_name from scenario import core as scenario_core @@ -118,9 +118,9 @@ def __init__( @classmethod def get_model_file(cls) -> str: - import gym_ignition_models + import gym_gz_models - return gym_ignition_models.get_model_file("iCubGazeboV2_5") + return gym_gz_models.get_model_file("iCubGazeboV2_5") class ICubGazeboSimpleCollisions(ICubGazeboABC): @@ -142,6 +142,6 @@ def __init__( @classmethod def get_model_file(cls) -> str: - import gym_ignition_models + import gym_gz_models - return gym_ignition_models.get_model_file("iCubGazeboSimpleCollisionsV2_5") + return gym_gz_models.get_model_file("iCubGazeboSimpleCollisionsV2_5") diff --git a/python/gym_ignition_environments/models/panda.py b/python/gym_gz_environments/models/panda.py similarity index 92% rename from python/gym_ignition_environments/models/panda.py rename to python/gym_gz_environments/models/panda.py index 4765b227f..ee3d3f139 100644 --- a/python/gym_ignition_environments/models/panda.py +++ b/python/gym_gz_environments/models/panda.py @@ -4,8 +4,8 @@ from typing import List -from gym_ignition.scenario import model_with_file, model_wrapper -from gym_ignition.utils.scenario import get_unique_model_name +from gym_gz.scenario import model_with_file, model_wrapper +from gym_gz.utils.scenario import get_unique_model_name from scenario import core as scenario @@ -77,6 +77,6 @@ def __init__( @classmethod def get_model_file(cls) -> str: - import gym_ignition_models + import gym_gz_models - return gym_ignition_models.get_model_file("panda") + return gym_gz_models.get_model_file("panda") diff --git a/python/gym_ignition_environments/models/pendulum.py b/python/gym_gz_environments/models/pendulum.py similarity index 85% rename from python/gym_ignition_environments/models/pendulum.py rename to python/gym_gz_environments/models/pendulum.py index d5218529e..8c7a11b8b 100644 --- a/python/gym_ignition_environments/models/pendulum.py +++ b/python/gym_gz_environments/models/pendulum.py @@ -4,8 +4,8 @@ from typing import List -from gym_ignition.scenario import model_with_file, model_wrapper -from gym_ignition.utils.scenario import get_unique_model_name +from gym_gz.scenario import model_with_file, model_wrapper +from gym_gz.utils.scenario import get_unique_model_name from scenario import core as scenario @@ -44,6 +44,6 @@ def __init__( @classmethod def get_model_file(cls) -> str: - import gym_ignition_models + import gym_gz_models - return gym_ignition_models.get_model_file("pendulum") + return gym_gz_models.get_model_file("pendulum") diff --git a/python/gym_ignition_environments/randomizers/__init__.py b/python/gym_gz_environments/randomizers/__init__.py similarity index 100% rename from python/gym_ignition_environments/randomizers/__init__.py rename to python/gym_gz_environments/randomizers/__init__.py diff --git a/python/gym_ignition_environments/randomizers/cartpole.py b/python/gym_gz_environments/randomizers/cartpole.py similarity index 93% rename from python/gym_ignition_environments/randomizers/cartpole.py rename to python/gym_gz_environments/randomizers/cartpole.py index 4e5a01a09..c9599d778 100644 --- a/python/gym_ignition_environments/randomizers/cartpole.py +++ b/python/gym_gz_environments/randomizers/cartpole.py @@ -5,13 +5,13 @@ import abc from typing import Union -from gym_ignition import randomizers, utils -from gym_ignition.randomizers import gazebo_env_randomizer -from gym_ignition.randomizers.gazebo_env_randomizer import MakeEnvCallable -from gym_ignition.randomizers.model.sdf import Distribution, Method, UniformParams -from gym_ignition.utils import misc -from gym_ignition_environments import tasks -from gym_ignition_environments.models import cartpole +from gym_gz import randomizers, utils +from gym_gz.randomizers import gazebo_env_randomizer +from gym_gz.randomizers.gazebo_env_randomizer import MakeEnvCallable +from gym_gz.randomizers.model.sdf import Distribution, Method, UniformParams +from gym_gz.utils import misc +from gym_gz_environments import tasks +from gym_gz_environments.models import cartpole from scenario import gazebo as scenario diff --git a/python/gym_ignition_environments/randomizers/cartpole_no_rand.py b/python/gym_gz_environments/randomizers/cartpole_no_rand.py similarity index 86% rename from python/gym_ignition_environments/randomizers/cartpole_no_rand.py rename to python/gym_gz_environments/randomizers/cartpole_no_rand.py index d5ae4076f..5539e38e5 100644 --- a/python/gym_ignition_environments/randomizers/cartpole_no_rand.py +++ b/python/gym_gz_environments/randomizers/cartpole_no_rand.py @@ -4,10 +4,10 @@ from typing import Union -from gym_ignition.randomizers import gazebo_env_randomizer -from gym_ignition.randomizers.gazebo_env_randomizer import MakeEnvCallable -from gym_ignition_environments import tasks -from gym_ignition_environments.models import cartpole +from gym_gz.randomizers import gazebo_env_randomizer +from gym_gz.randomizers.gazebo_env_randomizer import MakeEnvCallable +from gym_gz_environments import tasks +from gym_gz_environments.models import cartpole # Tasks that are supported by this randomizer. Used for type hinting. SupportedTasks = Union[ @@ -21,7 +21,7 @@ class CartpoleEnvNoRandomizations(gazebo_env_randomizer.GazeboEnvRandomizer): """ Dummy environment randomizer for cartpole tasks. - Check :py:class:`~gym_ignition_environments.randomizers.cartpole.CartpoleRandomizersMixin` + Check :py:class:`~gym_gz_environments.randomizers.cartpole.CartpoleRandomizersMixin` for an example that randomizes the task, the physics, and the model. """ diff --git a/python/gym_ignition_environments/tasks/__init__.py b/python/gym_gz_environments/tasks/__init__.py similarity index 100% rename from python/gym_ignition_environments/tasks/__init__.py rename to python/gym_gz_environments/tasks/__init__.py diff --git a/python/gym_ignition_environments/tasks/cartpole_continuous_balancing.py b/python/gym_gz_environments/tasks/cartpole_continuous_balancing.py similarity index 90% rename from python/gym_ignition_environments/tasks/cartpole_continuous_balancing.py rename to python/gym_gz_environments/tasks/cartpole_continuous_balancing.py index 7c131ba3e..8b64d6f49 100644 --- a/python/gym_ignition_environments/tasks/cartpole_continuous_balancing.py +++ b/python/gym_gz_environments/tasks/cartpole_continuous_balancing.py @@ -5,15 +5,16 @@ import abc from typing import Tuple -import gym +import gymnasium as gym import numpy as np -from gym_ignition.base import task -from gym_ignition.utils.typing import ( +from gym_gz.base import task +from gym_gz.utils.typing import ( Action, ActionSpace, Observation, ObservationSpace, Reward, + ResetReturn, ) from scenario import core as scenario @@ -45,7 +46,7 @@ def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: # Create the action space max_force = 50.0 # Nm action_space = gym.spaces.Box( - low=np.array([-max_force]), high=np.array([max_force]), dtype=np.float32 + low=np.array([-max_force]), high=np.array([max_force]), dtype=np.float64 ) # Configure reset limits @@ -59,12 +60,12 @@ def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: ) # Configure the reset space - self.reset_space = gym.spaces.Box(low=-high, high=high, dtype=np.float32) + self.reset_space = gym.spaces.Box(low=-high, high=high, dtype=np.float64) # Configure the observation space obs_high = high.copy() * 1.2 observation_space = gym.spaces.Box( - low=-obs_high, high=obs_high, dtype=np.float32 + low=-obs_high, high=obs_high, dtype=np.float64 ) return action_space, observation_space @@ -99,7 +100,7 @@ def get_observation(self) -> Observation: def get_reward(self) -> Reward: # Calculate the reward - reward = 1.0 if not self.is_done() else 0.0 + reward = 1.0 if not self.is_terminated() or not self.is_truncated() else 0.0 if self._reward_cart_at_center: @@ -115,15 +116,18 @@ def get_reward(self) -> Reward: return reward - def is_done(self) -> bool: + def is_terminated(self) -> bool: # Get the observation observation = self.get_observation() # The environment is done if the observation is outside its space - done = not self.reset_space.contains(observation) + terminated = not self.reset_space.contains(observation) - return done + return terminated + + def is_truncated(self) -> bool: + return False def reset_task(self) -> None: diff --git a/python/gym_ignition_environments/tasks/cartpole_continuous_swingup.py b/python/gym_gz_environments/tasks/cartpole_continuous_swingup.py similarity index 92% rename from python/gym_ignition_environments/tasks/cartpole_continuous_swingup.py rename to python/gym_gz_environments/tasks/cartpole_continuous_swingup.py index 12878ff87..5f09e82e3 100644 --- a/python/gym_ignition_environments/tasks/cartpole_continuous_swingup.py +++ b/python/gym_gz_environments/tasks/cartpole_continuous_swingup.py @@ -5,10 +5,10 @@ import abc from typing import Tuple -import gym +import gymnasium as gym import numpy as np -from gym_ignition.base import task -from gym_ignition.utils.typing import ( +from gym_gz.base import task +from gym_gz.utils.typing import ( Action, ActionSpace, Observation, @@ -45,7 +45,7 @@ def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: # Create the action space max_force = 200.0 # Nm action_space = gym.spaces.Box( - low=np.array([-max_force]), high=np.array([max_force]), dtype=np.float32 + low=np.array([-max_force]), high=np.array([max_force]), dtype=np.float64 ) # Configure reset limits @@ -59,12 +59,12 @@ def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: ) # Configure the reset space - self.reset_space = gym.spaces.Box(low=-high, high=high, dtype=np.float32) + self.reset_space = gym.spaces.Box(low=-high, high=high, dtype=np.float64) # Configure the observation space obs_high = high.copy() * 1.2 observation_space = gym.spaces.Box( - low=-obs_high, high=obs_high, dtype=np.float32 + low=-obs_high, high=obs_high, dtype=np.float64 ) return action_space, observation_space @@ -119,15 +119,18 @@ def get_reward(self) -> Reward: return reward - def is_done(self) -> bool: + def is_terminated(self) -> bool: # Get the observation observation = self.get_observation() # The environment is done if the observation is outside its space - done = not self.reset_space.contains(observation) + terminated = not self.reset_space.contains(observation) - return done + return terminated + + def is_truncated(self) -> bool: + return False def reset_task(self) -> None: diff --git a/python/gym_ignition_environments/tasks/cartpole_discrete_balancing.py b/python/gym_gz_environments/tasks/cartpole_discrete_balancing.py similarity index 90% rename from python/gym_ignition_environments/tasks/cartpole_discrete_balancing.py rename to python/gym_gz_environments/tasks/cartpole_discrete_balancing.py index 743a9b784..7039bb648 100644 --- a/python/gym_ignition_environments/tasks/cartpole_discrete_balancing.py +++ b/python/gym_gz_environments/tasks/cartpole_discrete_balancing.py @@ -5,10 +5,10 @@ import abc from typing import Tuple -import gym +import gymnasium as gym import numpy as np -from gym_ignition.base import task -from gym_ignition.utils.typing import ( +from gym_gz.base import task +from gym_gz.utils.typing import ( Action, ActionSpace, Observation, @@ -59,12 +59,12 @@ def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: ) # Configure the reset space - self.reset_space = gym.spaces.Box(low=-high, high=high, dtype=np.float32) + self.reset_space = gym.spaces.Box(low=-high, high=high, dtype=np.float64) # Configure the observation space obs_high = high.copy() * 1.2 observation_space = gym.spaces.Box( - low=-obs_high, high=obs_high, dtype=np.float32 + low=-obs_high, high=obs_high, dtype=np.float64 ) return action_space, observation_space @@ -99,7 +99,7 @@ def get_observation(self) -> Observation: def get_reward(self) -> Reward: # Calculate the reward - reward = 1.0 if not self.is_done() else 0.0 + reward = 1.0 if not self.is_terminated() or not self.is_truncated() else 0.0 if self._reward_cart_at_center: @@ -115,15 +115,18 @@ def get_reward(self) -> Reward: return reward - def is_done(self) -> bool: + def is_terminated(self) -> bool: # Get the observation observation = self.get_observation() # The environment is done if the observation is outside its space - done = not self.reset_space.contains(observation) + terminated = not self.reset_space.contains(observation) - return done + return terminated + + def is_truncated(self) -> bool: + return False def reset_task(self) -> None: diff --git a/python/gym_ignition_environments/tasks/pendulum_swingup.py b/python/gym_gz_environments/tasks/pendulum_swingup.py similarity index 90% rename from python/gym_ignition_environments/tasks/pendulum_swingup.py rename to python/gym_gz_environments/tasks/pendulum_swingup.py index 4019dd708..8464da2b4 100644 --- a/python/gym_ignition_environments/tasks/pendulum_swingup.py +++ b/python/gym_gz_environments/tasks/pendulum_swingup.py @@ -5,10 +5,10 @@ import abc from typing import Tuple -import gym +import gymnasium as gym import numpy as np -from gym_ignition.base import task -from gym_ignition.utils.typing import ( +from gym_gz.base import task +from gym_gz.utils.typing import ( Action, ActionSpace, Observation, @@ -35,12 +35,12 @@ def __init__(self, agent_rate: float, **kwargs): def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: action_space = gym.spaces.Box( - low=-self._max_torque, high=self._max_torque, shape=(1,), dtype=np.float32 + low=-self._max_torque, high=self._max_torque, shape=(1,), dtype=np.float64 ) high = np.array([1.0, 1.0, self._max_speed]) # cos(theta) # sin(theta) - observation_space = gym.spaces.Box(low=-high, high=high, dtype=np.float32) + observation_space = gym.spaces.Box(low=-high, high=high, dtype=np.float64) return action_space, observation_space @@ -75,7 +75,7 @@ def get_reward(self) -> Reward: # This environment is done only if the observation goes outside its limits. # Since it can happen only when velocity is too high, penalize this happening. - cost = 100.0 if self.is_done() else 0.0 + cost = 100.0 if self.is_terminated() else 0.0 # Get the model model = self.world.get_model(self.model_name) @@ -90,15 +90,18 @@ def get_reward(self) -> Reward: return Reward(-cost) - def is_done(self) -> bool: + def is_terminated(self) -> bool: # Get the observation observation = self.get_observation() # The environment is done if the observation is outside its space - done = not self.observation_space.contains(observation) + terminated = not self.observation_space.contains(observation) - return done + return terminated + + def is_truncated(self) -> bool: + return False def reset_task(self) -> None: diff --git a/scenario/bindings/__init__.py b/scenario/bindings/__init__.py index 5ad48a04a..a43f456be 100644 --- a/scenario/bindings/__init__.py +++ b/scenario/bindings/__init__.py @@ -129,7 +129,7 @@ def pre_import_gym() -> None: if spec is None: return - import gym + import gymnasium as gym def check_gazebo_installation() -> None: diff --git a/scenario/setup.cfg b/scenario/setup.cfg index 83dd3bfc8..b12122f22 100644 --- a/scenario/setup.cfg +++ b/scenario/setup.cfg @@ -22,7 +22,7 @@ project_urls = keywords = robotics gazebo - ignition + gz simulation physics multibody diff --git a/scenario/src/gazebo/include/scenario/gazebo/utils.h b/scenario/src/gazebo/include/scenario/gazebo/utils.h index 0c29c7c7a..9c4fe0506 100644 --- a/scenario/src/gazebo/include/scenario/gazebo/utils.h +++ b/scenario/src/gazebo/include/scenario/gazebo/utils.h @@ -153,7 +153,7 @@ namespace scenario::gazebo::utils { * Get a SDF model file from a Fuel URI. * * @note A valid URI has the following form: - * https://fuel.ignitionrobotics.org/openrobotics/models/model_name + * https://fuel.gazebosim.org/openrobotics/models/model_name * * @param URI A valid Fuel URI. * @param useCache Load the model from the local cache. diff --git a/scenario/src/gazebo/src/utils.cpp b/scenario/src/gazebo/src/utils.cpp index 51009ea1b..2c5f8f5fb 100644 --- a/scenario/src/gazebo/src/utils.cpp +++ b/scenario/src/gazebo/src/utils.cpp @@ -195,6 +195,7 @@ std::string utils::getModelFileFromFuel(const std::string& URI, const bool useCache) { std::string modelFilePath; + sError << "URI" << URI; using namespace gz; if (!useCache) { diff --git a/setup.cfg b/setup.cfg index acc92355c..10391f6de 100644 --- a/setup.cfg +++ b/setup.cfg @@ -3,8 +3,8 @@ # GNU Lesser General Public License v2.1 or any later version. [metadata] -name = gym_ignition -description = A toolkit for developing OpenAI Gym environments simulated with Ignition Gazebo. +name = gym_gz +description = A toolkit for developing OpenAI Gym environments simulated with Gz Sim. long_description = file: README.md long_description_content_type = text/markdown author = Diego Ferigo @@ -28,7 +28,7 @@ keywords = environment gazebo robotics - ignition + gz humanoid panda icub @@ -58,11 +58,11 @@ package_dir = =python python_requires = >=3.8 install_requires = - scenario >= 1.3.2.dev - gym >= 0.13.1 + scenario >= 0.1.dev1608+dirty + gymnasium >= 0.26.0 numpy scipy - gym_ignition_models + gym_gz_models lxml idyntree @@ -91,4 +91,4 @@ addopts = -rsxX -v --strict-markers testpaths = tests markers = scenario: Select the tests in the 'tests/test_scenario/' folder. - gym_ignition: Select the tests in the 'tests/test_gym_ignition/' folder. + gym_gz: Select the tests in the 'tests/test_gym_gz/' folder. diff --git a/tests/.python/test_environments_gazebowrapper.py b/tests/.python/test_environments_gazebowrapper.py index 9f5554bae..3d943ae34 100644 --- a/tests/.python/test_environments_gazebowrapper.py +++ b/tests/.python/test_environments_gazebowrapper.py @@ -2,9 +2,9 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -import gym +import gymnasium as gym import pytest -from gym_ignition.utils import logger +from gym_gz.utils import logger # Set verbosity logger.set_level(gym.logger.DEBUG) @@ -18,12 +18,12 @@ def template_run_environment(env_name): observation = env.observation_space.sample() assert observation.size > 0, "The sampled observation is empty" - observation = env.reset() + observation, info = env.reset(options={}) assert observation.size > 0, "The observation is empty" for _ in range(10): action = env.action_space.sample() - state, reward, done, _ = env.step(action) + state, reward, terminated, truncated, _ = env.step(action) assert state.size > 0, "The environment didn't return a valid state" env.close() diff --git a/tests/.python/test_joint_force.py b/tests/.python/test_joint_force.py index 8dacef426..26cf76cef 100644 --- a/tests/.python/test_joint_force.py +++ b/tests/.python/test_joint_force.py @@ -2,7 +2,7 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -from gym_ignition.base.robot.robot_joints import JointControlMode +from gym_gz.base.robot.robot_joints import JointControlMode from . import utils diff --git a/tests/.python/test_pendulum_wrt_ground_truth.py b/tests/.python/test_pendulum_wrt_ground_truth.py index d8c3b9f8b..073df2748 100644 --- a/tests/.python/test_pendulum_wrt_ground_truth.py +++ b/tests/.python/test_pendulum_wrt_ground_truth.py @@ -2,15 +2,16 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -import gym +import gymnasium as gym import numpy as np import pytest -from gym.envs import registry -from gym.envs.registration import register -from gym_ignition.robots.sim import gazebo, pybullet -from gym_ignition.tasks.pendulum_swingup import PendulumSwingUp -from gym_ignition.utils import logger -from gym_ignition.utils.typing import Observation, Reward, State +from gymnasium.envs import registry +from gymnasium.envs.registration import register +from gym_gz.robots.sim import gazebo, pybullet +from gym_gz.tasks.pendulum_swingup import PendulumSwingUp +from gym_gz.utils import logger +from gym_gz.utils.typing import Observation, Reward, State, +from typing import Dict # Set verbosity logger.set_level(gym.logger.DEBUG) @@ -68,7 +69,7 @@ def step(self, action: np.ndarray) -> State: return State((Observation(observation), Reward(0.0), False, {})) - def reset(self): + def reset(self, seed: int = None, options: Dict ={}, **kwargs): # Use set_state_from_obs pass @@ -84,10 +85,10 @@ def theta_from_obs(observation: np.ndarray) -> float: return np.math.atan2(sin_theta, cos_theta) -if "Pendulum-Ignition-PyTest-v0" not in [spec.id for spec in list(registry.all())]: +if "Pendulum-Gz-PyTest-v0" not in [spec.id for spec in list(registry.all())]: register( - id="Pendulum-Ignition-PyTest-v0", - entry_point="gym_ignition.runtimes.gazebo_runtime:GazeboRuntime", + id="Pendulum-Gz-PyTest-v0", + entry_point="gym_gz.runtimes.gazebo_runtime:GazeboRuntime", max_episode_steps=1000, kwargs={ "task_cls": PendulumSwingUp, @@ -104,7 +105,7 @@ def theta_from_obs(observation: np.ndarray) -> float: if "Pendulum-PyBullet-PyTest-v0" not in [spec.id for spec in list(registry.all())]: register( id="Pendulum-PyBullet-PyTest-v0", - entry_point="gym_ignition.runtimes.pybullet_runtime:PyBulletRuntime", + entry_point="gym_gz.runtimes.pybullet_runtime:PyBulletRuntime", max_episode_steps=1000, kwargs={ "task_cls": PendulumSwingUp, @@ -131,13 +132,12 @@ def template_pendulum_wrt_ground_truth(env_name: str, max_error_in_deg: float): # env.render('human') # time.sleep(5) - # Seed the environment - env.seed(42) for epoch in range(10): # Reset the environment logger.info("Resetting the environment") - observation = env.reset() + # Seed the environment + observation, _ = env.reset(seed=42, options={}) env_equation.set_state_from_obs(observation) # Initialize intermediate variables @@ -181,9 +181,9 @@ def template_pendulum_wrt_ground_truth(env_name: str, max_error_in_deg: float): @pytest.mark.parametrize( "env_name, max_error_in_deg", [ - ("Pendulum-Ignition-PyTest-v0", 3.0), + ("Pendulum-Gz-PyTest-v0", 3.0), ("Pendulum-PyBullet-PyTest-v0", 3.0), ], ) -def test_pendulum_ignition(env_name: str, max_error_in_deg: float): +def test_pendulum_gz(env_name: str, max_error_in_deg: float): template_pendulum_wrt_ground_truth(env_name, max_error_in_deg) diff --git a/tests/.python/test_rates.py b/tests/.python/test_rates.py index 53755f09f..ff7f4cbd7 100644 --- a/tests/.python/test_rates.py +++ b/tests/.python/test_rates.py @@ -8,8 +8,8 @@ import numpy as np import pytest -from gym_ignition.base.robot.robot_joints import JointControlMode -from gym_ignition.utils import logger +from gym_gz.base.robot.robot_joints import JointControlMode +from gym_gz.utils import logger from . import utils diff --git a/tests/.python/test_robot_controller.py b/tests/.python/test_robot_controller.py index 03946b4fa..c1b82ab37 100644 --- a/tests/.python/test_robot_controller.py +++ b/tests/.python/test_robot_controller.py @@ -2,10 +2,10 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -import gym +import gymnasium as gym import numpy as np -from gym_ignition import gympp_bindings as bindings -from gym_ignition.utils import logger, resource_finder +from gym_gz import gympp_bindings as bindings +from gym_gz.utils import logger, resource_finder def test_joint_controller(): @@ -40,7 +40,7 @@ def test_joint_controller(): # Create the gazebo wrapper num_of_iterations = int(physics_rate / agent_rate) - desired_rtf = float(np.finfo(np.float32).max) + desired_rtf = float(np.finfo(np.float64).max) gazebo = bindings.GazeboSimulator(num_of_iterations, desired_rtf, physics_rate) assert gazebo, "Failed to get the gazebo wrapper" @@ -51,9 +51,9 @@ def test_joint_controller(): world_ok = gazebo.setupGazeboWorld("DefaultEmptyWorld.world") assert world_ok, "Failed to initialize the gazebo world" - # Initialize the ignition gazebo wrapper (creates a paused simulation) + # Initialize the gz gazebo wrapper (creates a paused simulation) gazebo_initialized = gazebo.initialize() - assert gazebo_initialized, "Failed to initialize ignition gazebo" + assert gazebo_initialized, "Failed to initialize gz gazebo" # Insert the model model_ok = gazebo.insertModel(model_data, plugin_data) diff --git a/tests/.python/test_runtimes.py b/tests/.python/test_runtimes.py index f436fbb1d..ecd873267 100644 --- a/tests/.python/test_runtimes.py +++ b/tests/.python/test_runtimes.py @@ -2,23 +2,23 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -import gym +import gymnasium as gym import numpy as np import pytest -from gym.envs import registry -from gym.envs.registration import register -from gym_ignition.robots.sim import gazebo, pybullet -from gym_ignition.tasks.cartpole_discrete import CartPoleDiscrete -from gym_ignition.tasks.pendulum_swingup import PendulumSwingUp -from gym_ignition.utils import logger +from gymnasium.envs import registry +from gymnasium.envs.registration import register +from gym_gz.robots.sim import gazebo, pybullet +from gym_gz.tasks.cartpole_discrete import CartPoleDiscrete +from gym_gz.tasks.pendulum_swingup import PendulumSwingUp +from gym_gz.utils import logger # Set verbosity logger.set_level(gym.logger.DEBUG) -if "Pendulum-Ignition-PyTest-v0" not in [spec.id for spec in list(registry.all())]: +if "Pendulum-Gz-PyTest-v0" not in [spec.id for spec in list(registry.all())]: register( - id="Pendulum-Ignition-PyTest-v0", - entry_point="gym_ignition.runtimes.gazebo_runtime:GazeboRuntime", + id="Pendulum-Gz-PyTest-v0", + entry_point="gym_gz.runtimes.gazebo_runtime:GazeboRuntime", max_episode_steps=1000, kwargs={ "task_cls": PendulumSwingUp, @@ -35,7 +35,7 @@ if "Pendulum-PyBullet-PyTest-v0" not in [spec.id for spec in list(registry.all())]: register( id="Pendulum-PyBullet-PyTest-v0", - entry_point="gym_ignition.runtimes.pybullet_runtime:PyBulletRuntime", + entry_point="gym_gz.runtimes.pybullet_runtime:PyBulletRuntime", max_episode_steps=1000, kwargs={ "task_cls": PendulumSwingUp, @@ -49,12 +49,12 @@ }, ) -if "CartPoleDiscrete-Ignition-PyTest-v0" not in [ +if "CartPoleDiscrete-Gz-PyTest-v0" not in [ spec.id for spec in list(registry.all()) ]: register( - id="CartPoleDiscrete-Ignition-PyTest-v0", - entry_point="gym_ignition.runtimes.gazebo_runtime:GazeboRuntime", + id="CartPoleDiscrete-Gz-PyTest-v0", + entry_point="gym_gz.runtimes.gazebo_runtime:GazeboRuntime", max_episode_steps=500, kwargs={ "task_cls": CartPoleDiscrete, @@ -73,7 +73,7 @@ ]: register( id="CartPoleDiscrete-PyBullet-PyTest-v0", - entry_point="gym_ignition.runtimes.pybullet_runtime:PyBulletRuntime", + entry_point="gym_gz.runtimes.pybullet_runtime:PyBulletRuntime", max_episode_steps=500, kwargs={ "task_cls": CartPoleDiscrete, @@ -113,8 +113,8 @@ def template_compare_environments(env_name_a: str, env_name_b: str, max_error: f for epoch in range(10): # Reset the environments - observation_a = env_a.reset() - observation_b = env_b.reset() + observation_a, _ = env_a.reset() + observation_b, _ = env_b.reset() assert np.allclose( observation_a, observation_b @@ -157,9 +157,9 @@ def template_compare_environments(env_name_a: str, env_name_b: str, max_error: f @pytest.mark.parametrize( "env_name_a, env_name_b, max_error", [ - ("Pendulum-Ignition-PyTest-v0", "Pendulum-PyBullet-PyTest-v0", 0.05), + ("Pendulum-Gz-PyTest-v0", "Pendulum-PyBullet-PyTest-v0", 0.05), ( - "CartPoleDiscrete-Ignition-PyTest-v0", + "CartPoleDiscrete-Gz-PyTest-v0", "CartPoleDiscrete-PyBullet-PyTest-v0", 0.03, ), diff --git a/tests/.python/utils.py b/tests/.python/utils.py index 05d372f7f..a9e9dee30 100644 --- a/tests/.python/utils.py +++ b/tests/.python/utils.py @@ -8,9 +8,9 @@ import numpy as np import pybullet as p import pybullet_data -from gym_ignition import gympp_bindings as bindings -from gym_ignition.robots import gazebo_robot, sim -from gym_ignition.utils import resource_finder +from gym_gz import gympp_bindings as bindings +from gym_gz.robots import gazebo_robot, sim +from gym_gz.utils import resource_finder from pybullet_utils import bullet_client @@ -34,7 +34,7 @@ def __init__( self, physics_rate: float, iterations: int = int(1), - rtf=float(np.finfo(np.float32).max), + rtf=float(np.finfo(np.float64).max), ): self.simulator = bindings.GazeboSimulator(iterations, rtf, physics_rate) assert self.simulator diff --git a/tests/assets/worlds/fuel_support.sdf b/tests/assets/worlds/fuel_support.sdf index fe7b69257..2188e9d28 100644 --- a/tests/assets/worlds/fuel_support.sdf +++ b/tests/assets/worlds/fuel_support.sdf @@ -1,11 +1,11 @@ - + - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Ground Plane + https://fuel.gazebosim.org/OpenRobotics/models/Ground%20Plane - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + https://fuel.gazebosim.org/OpenRobotics/models/Sun 0 0 0.5 0 0 0 @@ -27,4 +27,4 @@ - + \ No newline at end of file diff --git a/tests/common/utils.py b/tests/common/utils.py index 100868bab..0fbc3837e 100644 --- a/tests/common/utils.py +++ b/tests/common/utils.py @@ -5,9 +5,9 @@ import dataclasses from typing import Tuple -import gym_ignition_models +import gym_gz_models import pytest -from gym_ignition.utils import misc +from gym_gz.utils import misc from scenario import gazebo as scenario @@ -77,7 +77,7 @@ def test_foo(default_world: Tuple[scenario.GazeboSimulator, scenario.World]): assert gazebo.initialize() world = gazebo.get_world().to_gazebo() - assert world.insert_model(gym_ignition_models.get_model_file("ground_plane")) + assert world.insert_model(gym_gz_models.get_model_file("ground_plane")) assert world.set_physics_engine(scenario.PhysicsEngine_dart) yield gazebo, world diff --git a/tests/test_gym_ignition/__init__.py b/tests/test_gym_gz/__init__.py similarity index 100% rename from tests/test_gym_ignition/__init__.py rename to tests/test_gym_gz/__init__.py diff --git a/tests/test_gym_ignition/test_inverse_kinematics.py b/tests/test_gym_gz/test_inverse_kinematics.py similarity index 95% rename from tests/test_gym_ignition/test_inverse_kinematics.py rename to tests/test_gym_gz/test_inverse_kinematics.py index 6f3e3004e..57e7a5fc4 100644 --- a/tests/test_gym_ignition/test_inverse_kinematics.py +++ b/tests/test_gym_gz/test_inverse_kinematics.py @@ -4,14 +4,14 @@ import pytest -pytestmark = pytest.mark.gym_ignition +pytestmark = pytest.mark.gym_gz from typing import Tuple import numpy as np -from gym_ignition.context.gazebo import controllers -from gym_ignition.rbd.idyntree import inverse_kinematics_nlp -from gym_ignition_environments import models +from gym_gz.context.gazebo import controllers +from gym_gz.rbd.idyntree import inverse_kinematics_nlp +from gym_gz_environments import models from scenario import gazebo as scenario_gazebo diff --git a/tests/test_gym_ignition/test_normalization.py b/tests/test_gym_gz/test_normalization.py similarity index 91% rename from tests/test_gym_ignition/test_normalization.py rename to tests/test_gym_gz/test_normalization.py index 6ce7e8076..149f428e8 100644 --- a/tests/test_gym_ignition/test_normalization.py +++ b/tests/test_gym_gz/test_normalization.py @@ -4,9 +4,9 @@ import pytest -pytestmark = pytest.mark.gym_ignition +pytestmark = pytest.mark.gym_gz -from gym_ignition.utils.math import denormalize, normalize +from gym_gz.utils.math import denormalize, normalize test_matrix = [ # Float, None diff --git a/tests/test_gym_ignition/test_reproducibility.py b/tests/test_gym_gz/test_reproducibility.py similarity index 64% rename from tests/test_gym_ignition/test_reproducibility.py rename to tests/test_gym_gz/test_reproducibility.py index ffbcc7118..d9596ac8d 100644 --- a/tests/test_gym_ignition/test_reproducibility.py +++ b/tests/test_gym_gz/test_reproducibility.py @@ -4,11 +4,11 @@ import pytest -pytestmark = pytest.mark.gym_ignition +pytestmark = pytest.mark.gym_gz -import gym -from gym_ignition.utils.logger import set_level -from gym_ignition_environments import randomizers +import gymnasium as gym +from gym_gz.utils.logger import set_level +from gym_gz_environments import randomizers # Set the verbosity set_level(gym.logger.DEBUG) @@ -16,8 +16,8 @@ def make_env(**kwargs) -> gym.Env: - import gym - import gym_ignition_environments + import gymnasium as gym + import gym_gz_environments return gym.make("CartPoleDiscreteBalancing-Gazebo-v0", **kwargs) @@ -35,15 +35,14 @@ def test_reproducibility(num_physics_rollouts: int): assert env1 != env2 - # Seed the environment env1.seed(42) env2.seed(42) for _ in range(5): - # Reset the environments - observation1 = env1.reset() - observation2 = env2.reset() + # Reset the environments with the same seed + observation1, _ = env1.reset(seed=42, options={}) + observation2, _ = env2.reset(seed=42, options={}) assert observation1 == pytest.approx(observation2) # Initialize returned values @@ -57,14 +56,19 @@ def test_reproducibility(num_physics_rollouts: int): assert action1 == pytest.approx(action2) # Step the environment - observation1, reward1, done1, info1 = env1.step(action1) - observation2, reward2, done2, info2 = env2.step(action2) + observation1, reward1, terminated1, truncated1, info1 = env1.step(action1) + observation2, reward2, terminated2, truncated2, info2 = env2.step(action2) - assert done1 == pytest.approx(done2) + assert truncated1 == pytest.approx(truncated2) + assert terminated1 == pytest.approx(terminated2) assert info1 == pytest.approx(info2) assert reward1 == pytest.approx(reward2) assert observation1 == pytest.approx(observation2) + # Check if the episode is done + done1 = terminated1 or truncated1 + done2 = terminated2 or truncated2 + done = done1 env1.close() diff --git a/tests/test_gym_ignition/test_sdf_randomizer.py b/tests/test_gym_gz/test_sdf_randomizer.py similarity index 95% rename from tests/test_gym_ignition/test_sdf_randomizer.py rename to tests/test_gym_gz/test_sdf_randomizer.py index c471d1402..120737479 100644 --- a/tests/test_gym_ignition/test_sdf_randomizer.py +++ b/tests/test_gym_gz/test_sdf_randomizer.py @@ -4,17 +4,17 @@ import pytest -pytestmark = pytest.mark.gym_ignition +pytestmark = pytest.mark.gym_gz -import gym_ignition_models -from gym_ignition.randomizers.model import sdf -from gym_ignition.randomizers.model.sdf import ( +import gym_gz_models +from gym_gz.randomizers.model import sdf +from gym_gz.randomizers.model.sdf import ( Distribution, GaussianParams, Method, UniformParams, ) -from gym_ignition.utils import misc +from gym_gz.utils import misc from lxml import etree from scenario import gazebo as scenario @@ -23,7 +23,7 @@ def test_sdf_randomizer(): # Get the URDF model - urdf_model = gym_ignition_models.get_model_file("cartpole") + urdf_model = gym_gz_models.get_model_file("cartpole") # Convert it to a SDF string sdf_model_string = scenario.urdffile_to_sdfstring(urdf_model) @@ -80,7 +80,7 @@ def test_sdf_randomizer(): def test_randomizer_reproducibility(): # Get the model - sdf_model = gym_ignition_models.get_model_file("ground_plane") + sdf_model = gym_gz_models.get_model_file("ground_plane") # Initialize the randomizers randomizer1 = sdf.SDFRandomizer(sdf_model=sdf_model) @@ -139,7 +139,7 @@ def test_randomizer_reproducibility(): def test_randomize_missing_element(): # Get the URDF model - urdf_model = gym_ignition_models.get_model_file("pendulum") + urdf_model = gym_gz_models.get_model_file("pendulum") # Convert it to a SDF string sdf_model_string = scenario.urdffile_to_sdfstring(urdf_model) @@ -197,7 +197,7 @@ def test_randomize_missing_element(): def test_full_panda_randomization(): # Get the URDF model - urdf_model = gym_ignition_models.get_model_file("panda") + urdf_model = gym_gz_models.get_model_file("panda") # Convert it to a SDF string sdf_model_string = scenario.urdffile_to_sdfstring(urdf_model) diff --git a/tests/test_scenario/test_contacts.py b/tests/test_scenario/test_contacts.py index 09223010f..7ff57e6c9 100644 --- a/tests/test_scenario/test_contacts.py +++ b/tests/test_scenario/test_contacts.py @@ -8,9 +8,9 @@ from typing import Callable -import gym_ignition_models +import gym_gz_models import numpy as np -from gym_ignition.utils import misc +from gym_gz.utils import misc from scenario import core from scenario import gazebo as scenario @@ -77,7 +77,7 @@ def test_cube_contact(gazebo: scenario.GazeboSimulator, get_model_str: Callable) assert world.set_physics_engine(scenario.PhysicsEngine_dart) # Insert the ground plane - assert world.insert_model(gym_ignition_models.get_model_file("ground_plane")) + assert world.insert_model(gym_gz_models.get_model_file("ground_plane")) assert len(world.model_names()) == 1 # Insert the cube @@ -150,7 +150,7 @@ def test_cube_multiple_contacts( assert world.set_physics_engine(scenario.PhysicsEngine_dart) # Insert the ground plane - assert world.insert_model(gym_ignition_models.get_model_file("ground_plane")) + assert world.insert_model(gym_gz_models.get_model_file("ground_plane")) assert len(world.model_names()) == 1 # Insert two cubes side to side with a 10cm gap diff --git a/tests/test_scenario/test_custom_controllers.py b/tests/test_scenario/test_custom_controllers.py index 0715d20f7..5e7dbcdde 100644 --- a/tests/test_scenario/test_custom_controllers.py +++ b/tests/test_scenario/test_custom_controllers.py @@ -6,9 +6,9 @@ pytestmark = pytest.mark.scenario -import gym_ignition_models +import gym_gz_models import numpy as np -from gym_ignition.context.gazebo import controllers +from gym_gz.context.gazebo import controllers from scenario import core from scenario import gazebo as scenario @@ -35,7 +35,7 @@ def test_computed_torque_fixed_base(gazebo: scenario.GazeboSimulator): assert world.set_physics_engine(scenario.PhysicsEngine_dart) # Get the panda urdf - panda_urdf = gym_ignition_models.get_model_file("panda") + panda_urdf = gym_gz_models.get_model_file("panda") # Insert the panda arm model_name = "panda" diff --git a/tests/test_scenario/test_gazebo_simulator.py b/tests/test_scenario/test_gazebo_simulator.py index 45fce9f62..1588aac8f 100644 --- a/tests/test_scenario/test_gazebo_simulator.py +++ b/tests/test_scenario/test_gazebo_simulator.py @@ -6,7 +6,7 @@ pytestmark = pytest.mark.scenario -import gym_ignition_models +import gym_gz_models from scenario import gazebo as scenario @@ -99,7 +99,7 @@ def test_paused_step(gazebo: scenario.GazeboSimulator): assert gazebo.initialize() world = gazebo.get_world().to_gazebo() - assert world.insert_model(gym_ignition_models.get_model_file("ground_plane")) + assert world.insert_model(gym_gz_models.get_model_file("ground_plane")) assert "ground_plane" in world.model_names() gazebo.run(paused=True) diff --git a/tests/test_scenario/test_ignition_fuel.py b/tests/test_scenario/test_gz_fuel.py similarity index 95% rename from tests/test_scenario/test_ignition_fuel.py rename to tests/test_scenario/test_gz_fuel.py index 734e1838c..bad6ba23c 100644 --- a/tests/test_scenario/test_ignition_fuel.py +++ b/tests/test_scenario/test_gz_fuel.py @@ -33,7 +33,7 @@ def test_download_model_from_fuel(gazebo: scenario.GazeboSimulator): # Download a model from Fuel (testing a name with spaces) model_name = "Electrical Box" model_sdf = scenario.get_model_file_from_fuel( - f"https://fuel.ignitionrobotics.org/openrobotics/models/{model_name}", False + f"https://fuel.gazebosim.org/OpenRobotics/models/{model_name}", False ) assert model_sdf diff --git a/tests/test_scenario/test_link_velocities.py b/tests/test_scenario/test_link_velocities.py index 90b709d9c..f80fb4a9d 100644 --- a/tests/test_scenario/test_link_velocities.py +++ b/tests/test_scenario/test_link_velocities.py @@ -8,9 +8,9 @@ from typing import Callable, Tuple -import gym_ignition_models +import gym_gz_models import numpy as np -from gym_ignition.utils.scenario import get_joint_positions_space +from gym_gz.utils.scenario import get_joint_positions_space from scipy.spatial.transform import Rotation from scenario import core @@ -49,7 +49,7 @@ def get_random_panda( gazebo: scenario.GazeboSimulator, world: scenario.World ) -> core.Model: - panda_urdf = gym_ignition_models.get_model_file("panda") + panda_urdf = gym_gz_models.get_model_file("panda") assert world.insert_model(panda_urdf) assert "panda" in world.model_names() @@ -261,9 +261,10 @@ def test_linear_acceleration( # By time to time there are steps where the acceleration becomes extremely high, # like 1000 m/s/s when the average is never exceeds 4 m/s/s. # We exclude those points. We should understand why this happens. - if (np.array(world_linear_acceleration()) > 100.0).any(): + if (np.absolute(np.array(world_linear_acceleration())) > 100.0).any(): continue + # Test the world acceleration (MIXED) assert world_linear_acceleration() == pytest.approx(world_acceleration, abs=0.5) diff --git a/tests/test_scenario/test_model.py b/tests/test_scenario/test_model.py index 9db832a8d..93297af3c 100644 --- a/tests/test_scenario/test_model.py +++ b/tests/test_scenario/test_model.py @@ -8,7 +8,7 @@ from typing import Tuple -import gym_ignition_models +import gym_gz_models import numpy as np from scenario import core @@ -23,7 +23,7 @@ def get_model( - gazebo: scenario.GazeboSimulator, gym_ignition_models_name: str + gazebo: scenario.GazeboSimulator, gym_gz_models_name: str ) -> scenario.Model: # Get the world and cast it to a Gazebo world @@ -31,14 +31,14 @@ def get_model( assert world.set_physics_engine(scenario.PhysicsEngine_dart) - model_urdf = gym_ignition_models.get_model_file(gym_ignition_models_name) + model_urdf = gym_gz_models.get_model_file(gym_gz_models_name) assert world.insert_model( - model_urdf, core.Pose_identity(), gym_ignition_models_name + model_urdf, core.Pose_identity(), gym_gz_models_name ) # Get the model and cast it to a Gazebo model - model = world.get_model(gym_ignition_models_name).to_gazebo() + model = world.get_model(gym_gz_models_name).to_gazebo() assert model.id() != 0 assert model.valid() @@ -52,12 +52,12 @@ def test_model_core_api(gazebo: scenario.GazeboSimulator): assert gazebo.initialize() - gym_ignition_model_name = "cartpole" - model = get_model(gazebo, gym_ignition_model_name) + gym_gz_model_name = "cartpole" + model = get_model(gazebo, gym_gz_model_name) assert model.id() != 0 assert model.valid() - assert model.name() == gym_ignition_model_name + assert model.name() == gym_gz_model_name assert len(model.link_names()) == model.nr_of_links() assert len(model.joint_names()) == model.nr_of_joints() @@ -73,8 +73,8 @@ def test_model_joints(gazebo: scenario.GazeboSimulator): assert gazebo.initialize() - gym_ignition_model_name = "panda" - model = get_model(gazebo, gym_ignition_model_name) + gym_gz_model_name = "panda" + model = get_model(gazebo, gym_gz_model_name) q = model.joint_positions() assert pytest.approx(q) == [0.0] * model.dofs() @@ -124,9 +124,9 @@ def test_model_base_pose(gazebo: scenario.GazeboSimulator): assert gazebo.initialize() - gym_ignition_model_name = "pendulum" - model = get_model(gazebo, gym_ignition_model_name) - assert gym_ignition_model_name in gazebo.get_world().model_names() + gym_gz_model_name = "pendulum" + model = get_model(gazebo, gym_gz_model_name) + assert gym_gz_model_name in gazebo.get_world().model_names() assert model.base_frame() == "support" # assert model.set_base_frame("support") # TODO: Not yet supported @@ -163,7 +163,7 @@ def test_model_base_velocity( # Get the simulator and the world gazebo, world = default_world - model_urdf = gym_ignition_models.get_model_file("pendulum") + model_urdf = gym_gz_models.get_model_file("pendulum") # Insert the first pendulum model1_name = "pendulum1" @@ -233,9 +233,9 @@ def test_model_references(gazebo: scenario.GazeboSimulator): assert gazebo.initialize() - gym_ignition_model_name = "cartpole" - model = get_model(gazebo, gym_ignition_model_name) - assert gym_ignition_model_name in gazebo.get_world().model_names() + gym_gz_model_name = "cartpole" + model = get_model(gazebo, gym_gz_model_name) + assert gym_gz_model_name in gazebo.get_world().model_names() assert model.set_joint_control_mode(core.JointControlMode_force) @@ -281,7 +281,7 @@ def test_history_of_joint_forces( gazebo, world = default_world # Insert a panda model - panda_urdf = gym_ignition_models.get_model_file("panda") + panda_urdf = gym_gz_models.get_model_file("panda") assert world.insert_model(panda_urdf) assert "panda" in world.model_names() diff --git a/tests/test_scenario/test_multi_world.py b/tests/test_scenario/test_multi_world.py index b5aaa8d95..be6efbbfc 100644 --- a/tests/test_scenario/test_multi_world.py +++ b/tests/test_scenario/test_multi_world.py @@ -6,9 +6,9 @@ pytestmark = pytest.mark.scenario -# import gym_ignition_models +# import gym_gz_models # import numpy as np -# from gym_ignition.utils import misc +# from gym_gz.utils import misc # from scenario import core as scenario_core from scenario import gazebo as scenario_gazebo @@ -170,7 +170,7 @@ def test_insert_world_multiple_calls(gazebo: scenario_gazebo.GazeboSimulator): # # sphere_urdf_string = utils.SphereURDF(restitution=0.8).urdf() # sphere_urdf = misc.string_to_file(sphere_urdf_string) -# ground_plane_urdf = gym_ignition_models.get_model_file(robot_name="ground_plane") +# ground_plane_urdf = gym_gz_models.get_model_file(robot_name="ground_plane") # # # Pose of the sphere # sphere_pose = scenario_core.Pose([0, 0, 0.5], [1, 0, 0, 0]) diff --git a/tests/test_scenario/test_pid_controllers.py b/tests/test_scenario/test_pid_controllers.py index 70d1a175e..e840f1b50 100644 --- a/tests/test_scenario/test_pid_controllers.py +++ b/tests/test_scenario/test_pid_controllers.py @@ -8,7 +8,7 @@ from typing import Tuple -import gym_ignition_models +import gym_gz_models import numpy as np from scenario import core @@ -41,7 +41,7 @@ def test_position_pid(default_world: Tuple[scenario.GazeboSimulator, scenario.Wo gazebo, world = default_world # Insert a panda model - panda_urdf = gym_ignition_models.get_model_file("panda") + panda_urdf = gym_gz_models.get_model_file("panda") assert world.insert_model(panda_urdf) assert "panda" in world.model_names() diff --git a/tests/test_scenario/test_velocity_direct.py b/tests/test_scenario/test_velocity_direct.py index 114f12d19..f1450e4dd 100644 --- a/tests/test_scenario/test_velocity_direct.py +++ b/tests/test_scenario/test_velocity_direct.py @@ -8,7 +8,7 @@ from typing import Tuple -import gym_ignition_models +import gym_gz_models import numpy as np from scenario import core as scenario_core @@ -29,7 +29,7 @@ def test_velocity_direct( gazebo, world = default_world # Get the URDF model - pendulum_urdf = gym_ignition_models.get_model_file("pendulum") + pendulum_urdf = gym_gz_models.get_model_file("pendulum") # Insert a pendulum model assert world.insert_model(pendulum_urdf) From 3c6b01338a3cbaf882712390cc5feb9ac7b510f2 Mon Sep 17 00:00:00 2001 From: Andrea Ostuni Date: Tue, 2 Jan 2024 21:18:28 +0100 Subject: [PATCH 4/5] migrate to new render API --- examples/python/launch_cartpole.py | 7 ++++++- python/gym_gz/runtimes/gazebo_runtime.py | 17 +++++++++++++++-- python/gym_gz/runtimes/realtime_runtime.py | 11 ++++++++--- tests/.python/test_pendulum_wrt_ground_truth.py | 10 ++++++---- tests/test_gym_gz/test_reproducibility.py | 4 ++-- 5 files changed, 37 insertions(+), 12 deletions(-) diff --git a/examples/python/launch_cartpole.py b/examples/python/launch_cartpole.py index 292b97438..276981572 100755 --- a/examples/python/launch_cartpole.py +++ b/examples/python/launch_cartpole.py @@ -48,6 +48,8 @@ def make_env_from_id(env_id: str, **kwargs) -> gym.Env: observation = env.reset(seed=42, options={}) # Initialize returned values + terminated = False + truncated = False done = False totalReward = 0 @@ -55,7 +57,10 @@ def make_env_from_id(env_id: str, **kwargs) -> gym.Env: # Execute a random action action = env.action_space.sample() - observation, reward, done, _ = env.step(action) + observation, reward, terminated, truncated, _ = env.step(action) + + # Check if the episode is terminated + done = terminated or truncated # Render the environment. # It is not required to call this in the loop if physics is not randomized. diff --git a/python/gym_gz/runtimes/gazebo_runtime.py b/python/gym_gz/runtimes/gazebo_runtime.py index ca04fcf10..2ef565002 100644 --- a/python/gym_gz/runtimes/gazebo_runtime.py +++ b/python/gym_gz/runtimes/gazebo_runtime.py @@ -46,6 +46,7 @@ def __init__( real_time_factor: float, physics_engine=scenario.PhysicsEngine_dart, world: str = None, + render_mode: Optional[str] = None, **kwargs, ): @@ -69,6 +70,9 @@ def __init__( self._world_sdf = world self._world_name = None + # Store the render mode + self.render_mode = render_mode + # Create the Task object task = task_cls(agent_rate=agent_rate, **kwargs) @@ -132,6 +136,10 @@ def step(self, action: Action) -> State: # Check truncation truncated = self.task.is_truncated() + # Render the environment + if self.render_mode == "human": + self.render() + # Get info info = self.task.get_info() @@ -158,10 +166,15 @@ def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> ResetReturn: logger.warn("The observation does not belong to the observation space") # Get info info = self.task.get_info() - return ResetReturn((Observation(observation), Info(info))) - def render(self, mode: str = "human", **kwargs) -> None: + # Render the environment + if self.render_mode == "human": + self.render() + + return ResetReturn((Observation(observation), Info(info))) + def render(self, **kwargs) -> None: + mode = self.render_mode if mode != "human": raise ValueError(f"Render mode '{mode}' not supported") diff --git a/python/gym_gz/runtimes/realtime_runtime.py b/python/gym_gz/runtimes/realtime_runtime.py index f75fe3431..0e407acee 100644 --- a/python/gym_gz/runtimes/realtime_runtime.py +++ b/python/gym_gz/runtimes/realtime_runtime.py @@ -4,7 +4,7 @@ from gym_gz.base import runtime, task from gym_gz.utils.typing import Action, Done, Info, Observation, State, Terminated, Truncated, ResetReturn, Dict - +from typing import Optional class RealTimeRuntime(runtime.Runtime): """ @@ -15,15 +15,19 @@ class RealTimeRuntime(runtime.Runtime): This class is not yet complete. """ - def __init__(self, task_cls: type, robot_cls: type, agent_rate: float, **kwargs): + def __init__(self, task_cls: type, robot_cls: type, agent_rate: float, render_mode: Optional[str] = None, **kwargs): # Build the environment task_object = task_cls(**kwargs) + # Check the task assert isinstance( task_object, task.Task ), "'task_cls' object must inherit from Task" + # Render mode + self.render_mode = render_mode + super().__init__(task=task_object, agent_rate=agent_rate) raise NotImplementedError @@ -80,7 +84,8 @@ def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> ResetReturn: return ResetReturn((observation, Info({}))) - def render(self, mode: str = "human", **kwargs) -> None: + def render(self, **kwargs) -> None: + mode = self.render_mode raise NotImplementedError def close(self) -> None: diff --git a/tests/.python/test_pendulum_wrt_ground_truth.py b/tests/.python/test_pendulum_wrt_ground_truth.py index 073df2748..7116d58d1 100644 --- a/tests/.python/test_pendulum_wrt_ground_truth.py +++ b/tests/.python/test_pendulum_wrt_ground_truth.py @@ -10,8 +10,8 @@ from gym_gz.robots.sim import gazebo, pybullet from gym_gz.tasks.pendulum_swingup import PendulumSwingUp from gym_gz.utils import logger -from gym_gz.utils.typing import Observation, Reward, State, -from typing import Dict +from gym_gz.utils.typing import Observation, Reward, State +from typing import Dict, Optional # Set verbosity logger.set_level(gym.logger.DEBUG) @@ -25,7 +25,7 @@ class PendulumEnv(gym.Env): metadata = {"render.modes": []} - def __init__(self): + def __init__(self, render_mode: Optional[str] = None): super().__init__() # Check the xacro pendulum model @@ -37,6 +37,7 @@ def __init__(self): self.dt = None # self.force = None + self.render_mode = render_mode self.theta = None self.theta_dot = None @@ -73,7 +74,8 @@ def reset(self, seed: int = None, options: Dict ={}, **kwargs): # Use set_state_from_obs pass - def render(self, mode="human", **kwargs): + def render(self, **kwargs): + mode = self.render_mode raise Exception("This runtime does not support rendering") def seed(self, seed=None): diff --git a/tests/test_gym_gz/test_reproducibility.py b/tests/test_gym_gz/test_reproducibility.py index d9596ac8d..9d220f2fe 100644 --- a/tests/test_gym_gz/test_reproducibility.py +++ b/tests/test_gym_gz/test_reproducibility.py @@ -35,8 +35,8 @@ def test_reproducibility(num_physics_rollouts: int): assert env1 != env2 - env1.seed(42) - env2.seed(42) + env1.unwrapped.seed(42) + env2.unwrapped.seed(42) for _ in range(5): From bc3de2987da95cbfa8869be8f105d712a89bf868 Mon Sep 17 00:00:00 2001 From: Andrea Ostuni Date: Wed, 3 Jan 2024 13:27:03 +0100 Subject: [PATCH 5/5] formatting and sorting --- examples/panda_pick_and_place.py | 4 +- examples/python/launch_cartpole.py | 2 +- python/gym_gz/__init__.py | 2 +- python/gym_gz/base/task.py | 4 +- python/gym_gz/randomizers/abc.py | 4 +- .../randomizers/gazebo_env_randomizer.py | 8 +++- python/gym_gz/runtimes/gazebo_runtime.py | 37 +++++++++++----- python/gym_gz/runtimes/realtime_runtime.py | 43 +++++++++++++++---- .../tasks/cartpole_continuous_balancing.py | 4 +- .../tasks/cartpole_continuous_swingup.py | 2 +- .../tasks/cartpole_discrete_balancing.py | 2 +- .../tasks/pendulum_swingup.py | 2 +- scenario/bindings/__init__.py | 4 +- .../.python/test_pendulum_wrt_ground_truth.py | 12 +++--- tests/.python/test_runtimes.py | 8 ++-- tests/test_gym_gz/test_reproducibility.py | 2 +- tests/test_scenario/test_link_velocities.py | 1 - tests/test_scenario/test_model.py | 4 +- 18 files changed, 90 insertions(+), 55 deletions(-) diff --git a/examples/panda_pick_and_place.py b/examples/panda_pick_and_place.py index 5b6c3d30e..b61458b39 100644 --- a/examples/panda_pick_and_place.py +++ b/examples/panda_pick_and_place.py @@ -246,9 +246,7 @@ def move_fingers( gazebo.run(paused=True) # Insert the Panda manipulator -panda = gym_gz_environments.models.panda.Panda( - world=world, position=[-0.1, 0, 1.0] -) +panda = gym_gz_environments.models.panda.Panda(world=world, position=[-0.1, 0, 1.0]) # Disable joint velocity limits _ = [j.to_gazebo().set_velocity_limit(1_000) for j in panda.joints()] diff --git a/examples/python/launch_cartpole.py b/examples/python/launch_cartpole.py index 276981572..e0f4ab747 100755 --- a/examples/python/launch_cartpole.py +++ b/examples/python/launch_cartpole.py @@ -20,8 +20,8 @@ def make_env_from_id(env_id: str, **kwargs) -> gym.Env: - import gymnasium as gym import gym_gz_environments + import gymnasium as gym return gym.make(env_id, **kwargs) diff --git a/python/gym_gz/__init__.py b/python/gym_gz/__init__.py index 8574e1da2..f1098003b 100644 --- a/python/gym_gz/__init__.py +++ b/python/gym_gz/__init__.py @@ -19,8 +19,8 @@ def initialize_verbosity() -> None: - import gymnasium as gym import gym_gz.utils.logger + import gymnasium as gym import scenario diff --git a/python/gym_gz/base/task.py b/python/gym_gz/base/task.py index 037d7cf6c..244ef7439 100644 --- a/python/gym_gz/base/task.py +++ b/python/gym_gz/base/task.py @@ -7,7 +7,6 @@ import gymnasium as gym import numpy as np -from gymnasium.utils import seeding from gym_gz.utils.typing import ( Action, ActionSpace, @@ -16,6 +15,7 @@ Reward, SeedList, ) +from gymnasium.utils import seeding from scenario import core @@ -208,7 +208,7 @@ def is_terminated(self) -> bool: Returns: True if the environment terminated, False otherwise. """ - + @abc.abstractmethod def is_truncated(self) -> bool: """ diff --git a/python/gym_gz/randomizers/abc.py b/python/gym_gz/randomizers/abc.py index 641c5a4a2..c45ff00c0 100644 --- a/python/gym_gz/randomizers/abc.py +++ b/python/gym_gz/randomizers/abc.py @@ -118,9 +118,7 @@ def randomize_model( class ModelDescriptionRandomizer(abc.ABC): @abc.abstractmethod - def randomize_model_description( - self, task: gym_gz.base.task.Task, **kwargs - ) -> str: + def randomize_model_description(self, task: gym_gz.base.task.Task, **kwargs) -> str: """ Randomize the model description. diff --git a/python/gym_gz/randomizers/gazebo_env_randomizer.py b/python/gym_gz/randomizers/gazebo_env_randomizer.py index c21ab29d3..65221eb4d 100644 --- a/python/gym_gz/randomizers/gazebo_env_randomizer.py +++ b/python/gym_gz/randomizers/gazebo_env_randomizer.py @@ -72,7 +72,9 @@ def __init__( # gym.Env methods # =============== - def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> typing.ResetReturn: + def reset( + self, seed: int = None, options: Dict = {}, **kwargs + ) -> typing.ResetReturn: # Reset the physics if self._physics_randomizer.physics_expired() and seed is None: @@ -94,7 +96,9 @@ def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> typing.Reset self._physics_randomizer.increase_rollout_counter() # Reset the task through the TaskRandomizer - self.randomize_task(task=self.env.unwrapped.task, gazebo=self.env.unwrapped.gazebo, **kwargs) + self.randomize_task( + task=self.env.unwrapped.task, gazebo=self.env.unwrapped.gazebo, **kwargs + ) ok_paused_run = self.env.unwrapped.gazebo.run(paused=True) diff --git a/python/gym_gz/runtimes/gazebo_runtime.py b/python/gym_gz/runtimes/gazebo_runtime.py index 2ef565002..d268f3cf4 100644 --- a/python/gym_gz/runtimes/gazebo_runtime.py +++ b/python/gym_gz/runtimes/gazebo_runtime.py @@ -2,16 +2,25 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -from typing import Optional, Dict +from typing import Dict, Optional +import gym_gz_models import gymnasium as gym import numpy as np - -import gym_gz_models from gym_gz import base from gym_gz.base import runtime from gym_gz.utils import logger -from gym_gz.utils.typing import Action, Info, Observation, Reward, SeedList, State, Terminated, Truncated, ResetReturn +from gym_gz.utils.typing import ( + Action, + Info, + Observation, + ResetReturn, + Reward, + SeedList, + State, + Terminated, + Truncated, +) from scenario import gazebo as scenario @@ -143,9 +152,17 @@ def step(self, action: Action) -> State: # Get info info = self.task.get_info() - return State((Observation(observation), Reward(reward), Terminated(terminated), Truncated(truncated), Info(info))) + return State( + ( + Observation(observation), + Reward(reward), + Terminated(terminated), + Truncated(truncated), + Info(info), + ) + ) - def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> ResetReturn: + def reset(self, seed: int = None, options: Dict = {}, **kwargs) -> ResetReturn: self.seed(seed) @@ -170,8 +187,8 @@ def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> ResetReturn: # Render the environment if self.render_mode == "human": self.render() - - return ResetReturn((Observation(observation), Info(info))) + + return ResetReturn((Observation(observation), Info(info))) def render(self, **kwargs) -> None: mode = self.render_mode @@ -282,9 +299,7 @@ def world(self) -> scenario.World: if self._world_sdf == "": # Insert the ground plane - ok_ground = world.insert_model( - gym_gz_models.get_model_file("ground_plane") - ) + ok_ground = world.insert_model(gym_gz_models.get_model_file("ground_plane")) if not ok_ground: raise RuntimeError("Failed to insert the ground plane") diff --git a/python/gym_gz/runtimes/realtime_runtime.py b/python/gym_gz/runtimes/realtime_runtime.py index 0e407acee..9168d1a40 100644 --- a/python/gym_gz/runtimes/realtime_runtime.py +++ b/python/gym_gz/runtimes/realtime_runtime.py @@ -2,10 +2,22 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -from gym_gz.base import runtime, task -from gym_gz.utils.typing import Action, Done, Info, Observation, State, Terminated, Truncated, ResetReturn, Dict from typing import Optional +from gym_gz.base import runtime, task +from gym_gz.utils.typing import ( + Action, + Dict, + Done, + Info, + Observation, + ResetReturn, + State, + Terminated, + Truncated, +) + + class RealTimeRuntime(runtime.Runtime): """ Implementation of :py:class:`~gym_gz.base.runtime.Runtime` for real-time @@ -15,7 +27,14 @@ class RealTimeRuntime(runtime.Runtime): This class is not yet complete. """ - def __init__(self, task_cls: type, robot_cls: type, agent_rate: float, render_mode: Optional[str] = None, **kwargs): + def __init__( + self, + task_cls: type, + robot_cls: type, + agent_rate: float, + render_mode: Optional[str] = None, + **kwargs + ): # Build the environment task_object = task_cls(**kwargs) @@ -27,7 +46,7 @@ def __init__(self, task_cls: type, robot_cls: type, agent_rate: float, render_mo # Render mode self.render_mode = render_mode - + super().__init__(task=task_object, agent_rate=agent_rate) raise NotImplementedError @@ -72,17 +91,25 @@ def step(self, action: Action) -> State: # Check termination terminated = self.task.is_terminated() - #Check truncation + # Check truncation truncated = self.task.is_truncated() - return State((observation, reward, Terminated(terminated), Truncated(truncated), Info({}))) + return State( + ( + observation, + reward, + Terminated(terminated), + Truncated(truncated), + Info({}), + ) + ) - def reset(self, seed: int = None, options : Dict = {}, **kwargs) -> ResetReturn: + def reset(self, seed: int = None, options: Dict = {}, **kwargs) -> ResetReturn: # Get the observation observation = self.task.get_observation() - return ResetReturn((observation, Info({}))) + return ResetReturn((observation, Info({}))) def render(self, **kwargs) -> None: mode = self.render_mode diff --git a/python/gym_gz_environments/tasks/cartpole_continuous_balancing.py b/python/gym_gz_environments/tasks/cartpole_continuous_balancing.py index 8b64d6f49..d4ed9a5e3 100644 --- a/python/gym_gz_environments/tasks/cartpole_continuous_balancing.py +++ b/python/gym_gz_environments/tasks/cartpole_continuous_balancing.py @@ -13,8 +13,8 @@ ActionSpace, Observation, ObservationSpace, - Reward, ResetReturn, + Reward, ) from scenario import core as scenario @@ -125,7 +125,7 @@ def is_terminated(self) -> bool: terminated = not self.reset_space.contains(observation) return terminated - + def is_truncated(self) -> bool: return False diff --git a/python/gym_gz_environments/tasks/cartpole_continuous_swingup.py b/python/gym_gz_environments/tasks/cartpole_continuous_swingup.py index 5f09e82e3..da1c92cc4 100644 --- a/python/gym_gz_environments/tasks/cartpole_continuous_swingup.py +++ b/python/gym_gz_environments/tasks/cartpole_continuous_swingup.py @@ -128,7 +128,7 @@ def is_terminated(self) -> bool: terminated = not self.reset_space.contains(observation) return terminated - + def is_truncated(self) -> bool: return False diff --git a/python/gym_gz_environments/tasks/cartpole_discrete_balancing.py b/python/gym_gz_environments/tasks/cartpole_discrete_balancing.py index 7039bb648..171201e3f 100644 --- a/python/gym_gz_environments/tasks/cartpole_discrete_balancing.py +++ b/python/gym_gz_environments/tasks/cartpole_discrete_balancing.py @@ -124,7 +124,7 @@ def is_terminated(self) -> bool: terminated = not self.reset_space.contains(observation) return terminated - + def is_truncated(self) -> bool: return False diff --git a/python/gym_gz_environments/tasks/pendulum_swingup.py b/python/gym_gz_environments/tasks/pendulum_swingup.py index 8464da2b4..0076ba0da 100644 --- a/python/gym_gz_environments/tasks/pendulum_swingup.py +++ b/python/gym_gz_environments/tasks/pendulum_swingup.py @@ -99,7 +99,7 @@ def is_terminated(self) -> bool: terminated = not self.observation_space.contains(observation) return terminated - + def is_truncated(self) -> bool: return False diff --git a/scenario/bindings/__init__.py b/scenario/bindings/__init__.py index a43f456be..273696f11 100644 --- a/scenario/bindings/__init__.py +++ b/scenario/bindings/__init__.py @@ -212,9 +212,7 @@ def import_gazebo() -> None: def create_home_dot_folder() -> None: # Make sure that the dot folder in the user's home exists - Path("~/.gz/gazebo").expanduser().mkdir( - mode=0o755, parents=True, exist_ok=True - ) + Path("~/.gz/gazebo").expanduser().mkdir(mode=0o755, parents=True, exist_ok=True) # =================== diff --git a/tests/.python/test_pendulum_wrt_ground_truth.py b/tests/.python/test_pendulum_wrt_ground_truth.py index 7116d58d1..a4640c23a 100644 --- a/tests/.python/test_pendulum_wrt_ground_truth.py +++ b/tests/.python/test_pendulum_wrt_ground_truth.py @@ -2,16 +2,17 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from typing import Dict, Optional + import gymnasium as gym import numpy as np import pytest -from gymnasium.envs import registry -from gymnasium.envs.registration import register from gym_gz.robots.sim import gazebo, pybullet from gym_gz.tasks.pendulum_swingup import PendulumSwingUp from gym_gz.utils import logger -from gym_gz.utils.typing import Observation, Reward, State -from typing import Dict, Optional +from gym_gz.utils.typing import Observation, Reward, State +from gymnasium.envs import registry +from gymnasium.envs.registration import register # Set verbosity logger.set_level(gym.logger.DEBUG) @@ -70,7 +71,7 @@ def step(self, action: np.ndarray) -> State: return State((Observation(observation), Reward(0.0), False, {})) - def reset(self, seed: int = None, options: Dict ={}, **kwargs): + def reset(self, seed: int = None, options: Dict = {}, **kwargs): # Use set_state_from_obs pass @@ -134,7 +135,6 @@ def template_pendulum_wrt_ground_truth(env_name: str, max_error_in_deg: float): # env.render('human') # time.sleep(5) - for epoch in range(10): # Reset the environment logger.info("Resetting the environment") diff --git a/tests/.python/test_runtimes.py b/tests/.python/test_runtimes.py index ecd873267..43716040e 100644 --- a/tests/.python/test_runtimes.py +++ b/tests/.python/test_runtimes.py @@ -5,12 +5,12 @@ import gymnasium as gym import numpy as np import pytest -from gymnasium.envs import registry -from gymnasium.envs.registration import register from gym_gz.robots.sim import gazebo, pybullet from gym_gz.tasks.cartpole_discrete import CartPoleDiscrete from gym_gz.tasks.pendulum_swingup import PendulumSwingUp from gym_gz.utils import logger +from gymnasium.envs import registry +from gymnasium.envs.registration import register # Set verbosity logger.set_level(gym.logger.DEBUG) @@ -49,9 +49,7 @@ }, ) -if "CartPoleDiscrete-Gz-PyTest-v0" not in [ - spec.id for spec in list(registry.all()) -]: +if "CartPoleDiscrete-Gz-PyTest-v0" not in [spec.id for spec in list(registry.all())]: register( id="CartPoleDiscrete-Gz-PyTest-v0", entry_point="gym_gz.runtimes.gazebo_runtime:GazeboRuntime", diff --git a/tests/test_gym_gz/test_reproducibility.py b/tests/test_gym_gz/test_reproducibility.py index 9d220f2fe..8a02db755 100644 --- a/tests/test_gym_gz/test_reproducibility.py +++ b/tests/test_gym_gz/test_reproducibility.py @@ -16,8 +16,8 @@ def make_env(**kwargs) -> gym.Env: - import gymnasium as gym import gym_gz_environments + import gymnasium as gym return gym.make("CartPoleDiscreteBalancing-Gazebo-v0", **kwargs) diff --git a/tests/test_scenario/test_link_velocities.py b/tests/test_scenario/test_link_velocities.py index f80fb4a9d..04be82024 100644 --- a/tests/test_scenario/test_link_velocities.py +++ b/tests/test_scenario/test_link_velocities.py @@ -264,7 +264,6 @@ def test_linear_acceleration( if (np.absolute(np.array(world_linear_acceleration())) > 100.0).any(): continue - # Test the world acceleration (MIXED) assert world_linear_acceleration() == pytest.approx(world_acceleration, abs=0.5) diff --git a/tests/test_scenario/test_model.py b/tests/test_scenario/test_model.py index 93297af3c..1a34d0d2b 100644 --- a/tests/test_scenario/test_model.py +++ b/tests/test_scenario/test_model.py @@ -33,9 +33,7 @@ def get_model( model_urdf = gym_gz_models.get_model_file(gym_gz_models_name) - assert world.insert_model( - model_urdf, core.Pose_identity(), gym_gz_models_name - ) + assert world.insert_model(model_urdf, core.Pose_identity(), gym_gz_models_name) # Get the model and cast it to a Gazebo model model = world.get_model(gym_gz_models_name).to_gazebo()