From 384d2976423f684ab970afeffae0cd5eae53edd6 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 26 Mar 2021 12:35:34 +0100 Subject: [PATCH 001/122] Update CMake to find Ignition Edifice --- CMakeLists.txt | 2 +- cmake/FindIgnitionDistribution.cmake | 1 + cmake/ImportTargetsEdifice.cmake | 73 ++++++++++++++++++++++++++++ 3 files changed, 75 insertions(+), 1 deletion(-) create mode 100644 cmake/ImportTargetsEdifice.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index ea0cea97a..7128af792 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -146,7 +146,7 @@ add_install_rpath_support( if(NOT IGNITION_DISTRIBUTION) include(FindIgnitionDistribution) - set(SUPPORTED_IGNITION_DISTRIBUTIONS "Dome" "Citadel") + set(SUPPORTED_IGNITION_DISTRIBUTIONS "Edifice" "Dome" "Citadel") foreach(distribution IN LISTS SUPPORTED_IGNITION_DISTRIBUTIONS) diff --git a/cmake/FindIgnitionDistribution.cmake b/cmake/FindIgnitionDistribution.cmake index d625ee11d..f2cde4e4e 100644 --- a/cmake/FindIgnitionDistribution.cmake +++ b/cmake/FindIgnitionDistribution.cmake @@ -7,6 +7,7 @@ # 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) macro(find_ignition_distribution) diff --git a/cmake/ImportTargetsEdifice.cmake b/cmake/ImportTargetsEdifice.cmake new file mode 100644 index 000000000..8099dbb8c --- /dev/null +++ b/cmake/ImportTargetsEdifice.cmake @@ -0,0 +1,73 @@ +include(AliasImportedTarget) + +# https://ignitionrobotics.org/docs/edifice/install#edifice-libraries + +alias_imported_target( + PACKAGE_ORIG sdformat11 + PACKAGE_DEST sdformat + TARGETS_ORIG sdformat11 + TARGETS_DEST sdformat + NAMESPACE_ORIG sdformat11 + NAMESPACE_DEST sdformat + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG ignition-gazebo5 + PACKAGE_DEST ignition-gazebo + TARGETS_ORIG ignition-gazebo5 core + TARGETS_DEST ignition-gazebo core + NAMESPACE_ORIG ignition-gazebo5 + NAMESPACE_DEST ignition-gazebo + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG ignition-common4 + PACKAGE_DEST ignition-common + TARGETS_ORIG ignition-common4 + TARGETS_DEST ignition-common + NAMESPACE_ORIG ignition-common4 + NAMESPACE_DEST ignition-common + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG ignition-sensors5-all + PACKAGE_DEST ignition-sensors-all + TARGETS_ORIG ignition-sensors5-all + TARGETS_DEST ignition-sensors-all + NAMESPACE_ORIG ignition-sensors5 + NAMESPACE_DEST ignition-sensors + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG ignition-rendering5 + PACKAGE_DEST ignition-rendering + TARGETS_ORIG ignition-rendering5 + TARGETS_DEST ignition-rendering + NAMESPACE_ORIG ignition-rendering5 + NAMESPACE_DEST ignition-rendering + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG ignition-gazebo5-rendering + PACKAGE_DEST ignition-gazebo-rendering + TARGETS_ORIG ignition-gazebo5-rendering + TARGETS_DEST ignition-gazebo-rendering + NAMESPACE_ORIG ignition-gazebo5 + NAMESPACE_DEST ignition-gazebo + REQUIRED TRUE + ) + +alias_imported_target( + PACKAGE_ORIG ignition-physics4 + PACKAGE_DEST ignition-physics + TARGETS_ORIG ignition-physics4 + TARGETS_DEST ignition-physics + NAMESPACE_ORIG ignition-physics4 + NAMESPACE_DEST ignition-physics + REQUIRED TRUE + ) From 347394b6d447e362463864d6540f1805d3062c03 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 26 Mar 2021 12:30:05 +0100 Subject: [PATCH 002/122] Use Ignition Edifice in CI for the nightly channel --- .docker/docker-compose.yml | 4 ++-- .github/workflows/cicd.yml | 9 +++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/.docker/docker-compose.yml b/.docker/docker-compose.yml index 8cd493a50..d98993ca0 100644 --- a/.docker/docker-compose.yml +++ b/.docker/docker-compose.yml @@ -40,7 +40,7 @@ services: build: args: from: diegoferigo/gym-ignition:base - ignition_codename: dome + ignition_codename: edifice CMAKE_BUILD_TYPE: Debug context: . dockerfile: cicd-devel.Dockerfile @@ -50,7 +50,7 @@ services: build: args: from: diegoferigo/gym-ignition:base - ignition_codename: dome + ignition_codename: edifice CMAKE_BUILD_TYPE: Release context: . dockerfile: cicd-devel.Dockerfile diff --git a/.github/workflows/cicd.yml b/.github/workflows/cicd.yml index 0e44ae267..92959675a 100644 --- a/.github/workflows/cicd.yml +++ b/.github/workflows/cicd.yml @@ -20,6 +20,7 @@ jobs: - User - Developer ignition: + - edifice - dome # - citadel @@ -163,13 +164,13 @@ jobs: # User installation - name: '[User] Create wheel' - if: matrix.type == 'User' && matrix.ignition == 'dome' + if: matrix.type == 'User' && matrix.ignition == 'edifice' shell: bash -i -e {0} run: pip3 wheel -w dist/ . # Note: calling "pip wheel" with "--global-option" forces dependencies to be build from their sdist. # Since it's very slow, we create the wheel from setup.py without isolation. - name: '[User] Create wheel' - if: matrix.type == 'User' && matrix.ignition != 'dome' + if: matrix.type == 'User' && matrix.ignition != 'edifice' shell: bash -i -e {0} run: | pip3 install \ @@ -265,8 +266,8 @@ jobs: - name: Publish package to PyPI if: | github.repository == 'robotology/gym-ignition' && matrix.type == 'User' && - ((github.event_name == 'release' && github.event.action == 'published' && matrix.ignition == 'dome') || - (github.event_name == 'push' && matrix.ignition == 'dome' && github.ref == 'refs/heads/devel')) + ((github.event_name == 'release' && github.event.action == 'published' && matrix.ignition == 'edifice') || + (github.event_name == 'push' && matrix.ignition == 'edifice' && github.ref == 'refs/heads/devel')) uses: pypa/gh-action-pypi-publish@master with: user: __token__ From 67a64bde9c0606bab35507862817b7c05b957c0a Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 26 Mar 2021 11:56:59 +0100 Subject: [PATCH 003/122] Fix installing Ignition dependencies when built from sources --- .github/workflows/cicd.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/cicd.yml b/.github/workflows/cicd.yml index 92959675a..926d0154d 100644 --- a/.github/workflows/cicd.yml +++ b/.github/workflows/cicd.yml @@ -110,7 +110,8 @@ jobs: cd /workspace/src wget -O - ${TAGS_YAML} | vcs import SYSTEM_VERSION=$(lsb_release -cs) - apt-get -y install $(sort -u $(find . -iname 'packages-'$SYSTEM_VERSION'.apt' -o -iname 'packages.apt') | tr '\n' ' ') + #apt-get -y install $(sort -u $(find . -iname 'packages-'$SYSTEM_VERSION'.apt' -o -iname 'packages.apt') | tr '\n' ' ') + apt-get -y install $(sort -u $(find . -iname 'packages-'$(lsb_release -cs)'.apt' -o -iname 'packages.apt') | grep -v -E "^libignition|^libsdformat") cd /workspace colcon graph colcon build \ From 58ccc0c3c1a8d98241316323e2e729ec6aaa1929 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 1 Apr 2021 17:22:44 +0200 Subject: [PATCH 004/122] Update documentation --- README.md | 2 +- docs/sphinx/info/faq.rst | 2 +- docs/sphinx/installation/support_policy.rst | 15 ++++++++------- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index d45365f74..9e5eef31b 100644 --- a/README.md +++ b/README.md @@ -103,7 +103,7 @@ our simulations, visit the _Motivations_ section of the [website](https://roboto ## Setup -1. Install the latest Ignition suite following the [official instructions](https://ignitionrobotics.org/docs/dome). +1. Install the Ignition suite following the [official instructions](https://ignitionrobotics.org/docs/edifice). 1. Execute `pip install gym-ignition`, preferably in a virtual environment. **Note**: `gym-ignition` currently only supports the latest version of the ignition suite. For more information on supported versions please refer to the [Support Policy](https://robotology.github.io/gym-ignition/master/installation/support_policy.html). diff --git a/docs/sphinx/info/faq.rst b/docs/sphinx/info/faq.rst index ff0519d0f..04ef2b6ce 100644 --- a/docs/sphinx/info/faq.rst +++ b/docs/sphinx/info/faq.rst @@ -24,7 +24,7 @@ On GNU/Linux distributions that ship an old OpenGL version, the GUI could fail t error like *Unable to create the rendering window*. The reason is that Ignition Gazebo has `ogre-next `_ (also known as ogre2) as default rendering engine, and it requires OpenGL greater than 3.3. -You can find more details `here `_. +You can find more details `here `_. The workaround we recommend is modifying the file ``~/.ignition/gazebo/gui.config`` as follows: diff --git a/docs/sphinx/installation/support_policy.rst b/docs/sphinx/installation/support_policy.rst index 37b020370..b2270f13b 100644 --- a/docs/sphinx/installation/support_policy.rst +++ b/docs/sphinx/installation/support_policy.rst @@ -12,15 +12,16 @@ We do not yet provide direct support to other operating systems. The table below recaps the project requirements of the :ref:`Stable ` and :ref:`Nightly ` channels: -+-------------+-----------------+--------+------------------+----------+------------+---------+ -| Channel | C++ | Python | Ignition | Ubuntu | macOS [*]_ | Windows | -+=============+=================+========+==================+==========+============+=========+ -| **Stable** | >= gcc8, clang6 | >= 3.8 | `Dome`_ (binary) | >= 20.04 | No | No | -+-------------+-----------------+--------+------------------+----------+------------+---------+ -| **Nightly** | >= gcc8, clang6 | >= 3.8 | `Dome`_ (source) | >= 20.04 | No | No | -+-------------+-----------------+--------+------------------+----------+------------+---------+ ++-------------+-----------------+--------+---------------------+----------+------------+---------+ +| Channel | C++ | Python | Ignition | Ubuntu | macOS [*]_ | Windows | ++=============+=================+========+=====================+==========+============+=========+ +| **Stable** | >= gcc8, clang6 | >= 3.8 | `Dome`_ (binary) | >= 20.04 | No | No | ++-------------+-----------------+--------+---------------------+----------+------------+---------+ +| **Nightly** | >= gcc8, clang6 | >= 3.8 | `Edifice`_ (source) | >= 20.04 | No | No | ++-------------+-----------------+--------+---------------------+----------+------------+---------+ .. _`Dome`: https://ignitionrobotics.org/docs/dome/install +.. _`Edifice`: https://ignitionrobotics.org/docs/edifice/install .. [*] Ignition officially supports macOS and also ``gym-ignition`` could be installed on this platform. However, we do not currently test this configuration and we cannot guarantee support. From eee866d8bcf16673db49b48d171a6d61b9d8a642 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 26 Mar 2021 13:01:57 +0100 Subject: [PATCH 005/122] Use interactive bash session also when calling pytest Otherwise the environment does not source the setup.sh script of Ignition --- .github/workflows/cicd.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/cicd.yml b/.github/workflows/cicd.yml index 926d0154d..f2755e4e3 100644 --- a/.github/workflows/cicd.yml +++ b/.github/workflows/cicd.yml @@ -193,11 +193,13 @@ jobs: # ============ - name: '[ScenarI/O] Python Tests' + shell: bash -i -e {0} run: | cd tests pytest -m "scenario" - name: '[ScenarI/O] Python Tests with Valgrind' + shell: bash -i -e {0} if: failure() run: | apt-get install -y --no-install-recommends valgrind @@ -206,11 +208,13 @@ jobs: valgrind --log-file=/tmp/valgrind.log pytest -s -m "scenario" || colour-valgrind -t /tmp/valgrind.log - name: '[gym_ignition] Python Tests' + shell: bash -i -e {0} run: | cd tests pytest -m "gym_ignition" - name: '[gym_ignition] Python Tests with Valgrind' + shell: bash -i -e {0} if: failure() run: | pip3 install colour-valgrind From fd93ac5b7f04a6c0301c783a3d94d926bd6f89e2 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 27 Apr 2021 23:28:14 +0200 Subject: [PATCH 006/122] Align Physics system (#318) * Vendor Physics system https://github.com/ignitionrobotics/ign-gazebo/commit/1924de9b2286a83424e96ca0d9f99903b4864070 * Custom Physics features * Disable using Ignition Dome in CI/CD * Enable Physics system profiling * Use upstream components to reset the base velocity * Fix updating the GUI state with paused run --- .github/workflows/cicd.yml | 4 +- cpp/scenario/CMakeLists.txt | 3 + cpp/scenario/gazebo/CMakeLists.txt | 1 - .../gazebo/components/WorldVelocityCmd.h | 65 - .../gazebo/include/scenario/gazebo/helpers.h | 22 +- cpp/scenario/gazebo/src/GazeboSimulator.cpp | 36 +- cpp/scenario/gazebo/src/Model.cpp | 92 +- cpp/scenario/gazebo/src/helpers.cpp | 39 +- cpp/scenario/plugins/Physics/CMakeLists.txt | 8 +- .../plugins/Physics/EntityFeatureMap.hh | 313 ++ cpp/scenario/plugins/Physics/Physics.cc | 2688 +++++++++++++++++ cpp/scenario/plugins/Physics/Physics.cpp | 2556 ---------------- cpp/scenario/plugins/Physics/Physics.h | 143 - cpp/scenario/plugins/Physics/Physics.hh | 95 + 14 files changed, 3207 insertions(+), 2858 deletions(-) delete mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h create mode 100644 cpp/scenario/plugins/Physics/EntityFeatureMap.hh create mode 100644 cpp/scenario/plugins/Physics/Physics.cc delete mode 100644 cpp/scenario/plugins/Physics/Physics.cpp delete mode 100644 cpp/scenario/plugins/Physics/Physics.h create mode 100644 cpp/scenario/plugins/Physics/Physics.hh diff --git a/.github/workflows/cicd.yml b/.github/workflows/cicd.yml index e2ff819ec..11ec19dd4 100644 --- a/.github/workflows/cicd.yml +++ b/.github/workflows/cicd.yml @@ -20,9 +20,9 @@ jobs: - User - Developer ignition: - - edifice - - dome # - citadel + # - dome + - edifice env: CCACHE_DIR: ${{ github.workspace }}/.ccache diff --git a/cpp/scenario/CMakeLists.txt b/cpp/scenario/CMakeLists.txt index 1f1cffe53..7721c9f77 100644 --- a/cpp/scenario/CMakeLists.txt +++ b/cpp/scenario/CMakeLists.txt @@ -9,6 +9,9 @@ set(SCENARIO_PRIVATE_DEPENDENCIES "") if(SCENARIO_USE_IGNITION) + option(ENABLE_PROFILER "Enable Ignition Profiler" OFF) + mark_as_advanced(ENABLE_PROFILER) + add_subdirectory(gazebo) add_subdirectory(plugins) add_subdirectory(controllers) diff --git a/cpp/scenario/gazebo/CMakeLists.txt b/cpp/scenario/gazebo/CMakeLists.txt index 06a84761a..545f6f1a7 100644 --- a/cpp/scenario/gazebo/CMakeLists.txt +++ b/cpp/scenario/gazebo/CMakeLists.txt @@ -34,7 +34,6 @@ set(EXTRA_COMPONENTS_PUBLIC_HEADERS include/scenario/gazebo/components/BaseWorldAccelerationTarget.h include/scenario/gazebo/components/JointControlMode.h include/scenario/gazebo/components/JointController.h - include/scenario/gazebo/components/WorldVelocityCmd.h include/scenario/gazebo/components/JointPositionTarget.h include/scenario/gazebo/components/JointVelocityTarget.h include/scenario/gazebo/components/JointAccelerationTarget.h diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h deleted file mode 100644 index 13a8d7490..000000000 --- a/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This project is dual licensed under LGPL v2.1+ or Apache License. - * - * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - * - * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_H -#define IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_H - -#include - -#include -#include - -#include "ignition/gazebo/components/Component.hh" -#include - -namespace ignition::gazebo { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { - struct WorldVelocity - { - math::Vector3d linear; - math::Vector3d angular; - - bool operator==(const WorldVelocity& other) const - { - return this->linear == other.linear - && this->angular == other.angular; - } - }; - - namespace components { - /// \brief A component type that contains commanded velocity of - /// an entity in the world frame represented by - /// ignition::math::Vector3d. - using WorldVelocityCmd = - Component; - IGN_GAZEBO_REGISTER_COMPONENT( - "ign_gazebo_components.WorldVelocityCmdTag", - WorldVelocityCmd) - } // namespace components - } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace ignition::gazebo -#endif // IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h index 138d63def..6c8958001 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h @@ -165,17 +165,17 @@ namespace scenario::gazebo::utils { sdf::JointType toSdf(const scenario::core::JointType type); scenario::core::JointType fromSdf(const sdf::JointType sdfType); - std::pair - fromModelToBaseVelocity(const ignition::math::Vector3d& linModelVelocity, - const ignition::math::Vector3d& angModelVelocity, - const ignition::math::Pose3d& M_H_B, - const ignition::math::Quaterniond& W_R_B); - - std::pair - fromBaseToModelVelocity(const ignition::math::Vector3d& linBaseVelocity, - const ignition::math::Vector3d& angBaseVelocity, - const ignition::math::Pose3d& M_H_B, - const ignition::math::Quaterniond& W_R_B); + 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); + + 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); std::shared_ptr getParentWorld(const GazeboEntity& gazeboEntity); diff --git a/cpp/scenario/gazebo/src/GazeboSimulator.cpp b/cpp/scenario/gazebo/src/GazeboSimulator.cpp index 05049143f..4b08d9d67 100644 --- a/cpp/scenario/gazebo/src/GazeboSimulator.cpp +++ b/cpp/scenario/gazebo/src/GazeboSimulator.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -233,12 +234,45 @@ bool GazeboSimulator::run(const bool paused) iterations = 1; } + // Recent versions of Ignition 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 + // setting them as changed. + if (paused) { + // Get the singleton + auto& ecmSingleton = + scenario::plugins::gazebo::ECMSingleton::Instance(); + + // Process all worlds + for (const auto& worldName : this->worldNames()) { + assert(ecmSingleton.hasWorld(worldName)); + assert(ecmSingleton.valid(worldName)); + assert(ecmSingleton.getECM(worldName)); + + // Get the ECM + auto* ecm = ecmSingleton.getECM(worldName); + + // Mark all all entities with Pose component as Changed + ecm->Each( + [&](const ignition::gazebo::Entity& entity, + ignition::gazebo::components::Pose*) -> bool { + ecm->SetChanged( + entity, + ignition::gazebo::components::Pose::typeId, + ignition::gazebo::ComponentState::OneTimeChange); + return true; + }); + } + } + + // Paused simulation run if (paused && !server->RunOnce(/*paused=*/true)) { sError << "The server couldn't execute the paused step" << std::endl; return false; } - // Run the simulation + // Unpaused simulation run if (!paused && !server->Run(/*blocking=*/deterministic, /*iterations=*/iterations, diff --git a/cpp/scenario/gazebo/src/Model.cpp b/cpp/scenario/gazebo/src/Model.cpp index 7f1101713..6d9c7b545 100644 --- a/cpp/scenario/gazebo/src/Model.cpp +++ b/cpp/scenario/gazebo/src/Model.cpp @@ -34,15 +34,16 @@ #include "scenario/gazebo/components/BaseWorldVelocityTarget.h" #include "scenario/gazebo/components/JointControllerPeriod.h" #include "scenario/gazebo/components/Timestamp.h" -#include "scenario/gazebo/components/WorldVelocityCmd.h" #include "scenario/gazebo/exceptions.h" #include "scenario/gazebo/helpers.h" #include #include #include +#include #include #include +#include #include #include #include @@ -300,49 +301,13 @@ bool Model::resetBaseOrientation(const std::array& orientation) bool Model::resetBaseWorldLinearVelocity(const std::array& linear) { - // Check if the velocity was not already reset in this simulation run, - // otherwise the previous target would get overridden - if (!this->m_ecm->EntityHasComponentType( - this->m_entity, - ignition::gazebo::components::WorldVelocityCmd::typeId)) { + // Note: there could be a rigid transformation between the base frame and + // the canonical frame. The Physics system processes velocity commands + // in the canonical frame, but this method receives velocity commands + // of in the base frame (all expressed in world coordinates). + // Therefore, we need to compute the base linear velocity from the + // base frame to the canonical frame. - return this->resetBaseWorldVelocity(linear, - this->baseWorldAngularVelocity()); - } - - // Get the existing cmd - const auto& velocityCmd = utils::getExistingComponentData< - ignition::gazebo::components::WorldVelocityCmd>(m_ecm, m_entity); - - // Override only the linear velocity - return this->resetBaseWorldVelocity( - linear, utils::fromIgnitionVector(velocityCmd.angular)); -} - -bool Model::resetBaseWorldAngularVelocity(const std::array& angular) -{ - // Check if the velocity was not already reset in this simulation run, - // otherwise the previous target would get overridden - if (!this->m_ecm->EntityHasComponentType( - this->m_entity, - ignition::gazebo::components::WorldVelocityCmd::typeId)) { - - return this->resetBaseWorldVelocity(this->baseWorldLinearVelocity(), - angular); - } - - // Get the existing cmd - const auto& velocityCmd = utils::getExistingComponentData< - ignition::gazebo::components::WorldVelocityCmd>(m_ecm, m_entity); - - // Override only the angular velocity - return this->resetBaseWorldVelocity( - utils::fromIgnitionVector(velocityCmd.linear), angular); -} - -bool Model::resetBaseWorldVelocity(const std::array& linear, - const std::array& angular) -{ // Get the entity of the canonical (base) link const auto canonicalLinkEntity = m_ecm->EntityByComponents( ignition::gazebo::components::Link(), @@ -359,23 +324,40 @@ bool Model::resetBaseWorldVelocity(const std::array& linear, const auto& W_R_B = utils::toIgnitionQuaternion( this->getLink(this->baseFrame())->orientation()); - // Create the new model velocity - ignition::gazebo::WorldVelocity baseWorldVelocity; - - // Compute the mixed velocity of the base link - std::tie(baseWorldVelocity.linear, baseWorldVelocity.angular) = - utils::fromModelToBaseVelocity(utils::toIgnitionVector3(linear), - utils::toIgnitionVector3(angular), - M_H_B, - W_R_B); + // Compute the linear part of the base link mixed velocity + const ignition::math::Vector3d baseLinearWorldVelocity = + utils::fromModelToBaseLinearVelocity( + utils::toIgnitionVector3(linear), + utils::toIgnitionVector3(this->baseWorldAngularVelocity()), + M_H_B, + W_R_B); // Store the new velocity - utils::setComponentData( - m_ecm, m_entity, baseWorldVelocity); + utils::setComponentData( + m_ecm, m_entity, baseLinearWorldVelocity); + + return true; +} + +bool Model::resetBaseWorldAngularVelocity(const std::array& angular) +{ + // Note: the angular part of the velocity does not change between the base + // 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)); return true; } +bool Model::resetBaseWorldVelocity(const std::array& linear, + const std::array& angular) +{ + return this->resetBaseWorldLinearVelocity(linear) + && this->resetBaseWorldAngularVelocity(angular); +} + bool Model::valid() const { return this->validEntity() && pImpl->model.Valid(*m_ecm); @@ -1050,7 +1032,7 @@ std::array Model::baseWorldLinearVelocity() const this->getLink(this->baseFrame())->worldAngularVelocity()); // Convert the base velocity to the model mixed velocity - auto [modelLinearVelocity, _] = utils::fromBaseToModelVelocity( // + const auto& modelLinearVelocity = utils::fromBaseToModelLinearVelocity( // canonicalLinkLinearVelocity, canonicalLinkAngularVelocity, M_H_B, diff --git a/cpp/scenario/gazebo/src/helpers.cpp b/cpp/scenario/gazebo/src/helpers.cpp index 5ef22dea2..22ea23b27 100644 --- a/cpp/scenario/gazebo/src/helpers.cpp +++ b/cpp/scenario/gazebo/src/helpers.cpp @@ -504,46 +504,41 @@ scenario::core::JointType utils::fromSdf(const sdf::JointType sdfType) return type; } -std::pair -utils::fromModelToBaseVelocity(const ignition::math::Vector3d& linModelVelocity, - const ignition::math::Vector3d& angModelVelocity, - const ignition::math::Pose3d& M_H_B, - const ignition::math::Quaterniond& W_R_B) +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) { - ignition::math::Vector3d linBaseVelocity; - const ignition::math::Vector3d& angBaseVelocity = angModelVelocity; - // 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(); auto B_o_M = -B_R_M * M_o_B; // Compute the base linear velocity - linBaseVelocity = linModelVelocity - angModelVelocity.Cross(W_R_B * B_o_M); + const ignition::math::Vector3d linBaseVelocity = + linModelVelocity - angModelVelocity.Cross(W_R_B * B_o_M); - // Return the mixed velocity of the base - return {linBaseVelocity, angBaseVelocity}; + // Return the linear part of the mixed velocity of the base + return linBaseVelocity; } -std::pair -utils::fromBaseToModelVelocity(const ignition::math::Vector3d& linBaseVelocity, - const ignition::math::Vector3d& angBaseVelocity, - const ignition::math::Pose3d& M_H_B, - const ignition::math::Quaterniond& W_R_B) +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) { - ignition::math::Vector3d linModelVelocity; - const ignition::math::Vector3d& angModelVelocity = angBaseVelocity; - // 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 - linModelVelocity = + const ignition::math::Vector3d linModelVelocity = linBaseVelocity - angBaseVelocity.Cross(W_R_B * B_R_M * M_o_B); - // Return the mixed velocity of the model - return {linModelVelocity, angModelVelocity}; + // Return the linear part of the mixed velocity of the model + return linModelVelocity; } std::shared_ptr utils::getParentWorld(const GazeboEntity& gazeboEntity) diff --git a/cpp/scenario/plugins/Physics/CMakeLists.txt b/cpp/scenario/plugins/Physics/CMakeLists.txt index 80fca4065..0ca0be299 100644 --- a/cpp/scenario/plugins/Physics/CMakeLists.txt +++ b/cpp/scenario/plugins/Physics/CMakeLists.txt @@ -27,8 +27,8 @@ # ============= add_library(PhysicsSystem SHARED - Physics.h - Physics.cpp) + Physics.hh + Physics.cc) target_link_libraries(PhysicsSystem PUBLIC @@ -41,6 +41,10 @@ target_link_libraries(PhysicsSystem target_include_directories(PhysicsSystem PRIVATE $) +if(ENABLE_PROFILER) + target_compile_definitions(PhysicsSystem PRIVATE "IGN_PROFILER_ENABLE=1") +endif() + # =================== # Install the targets # =================== diff --git a/cpp/scenario/plugins/Physics/EntityFeatureMap.hh b/cpp/scenario/plugins/Physics/EntityFeatureMap.hh new file mode 100644 index 000000000..8008f478d --- /dev/null +++ b/cpp/scenario/plugins/Physics/EntityFeatureMap.hh @@ -0,0 +1,313 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_ +#define IGNITION_GAZEBO_SYSTEMS_PHYSICS_ENTITY_FEATURE_MAP_HH_ + +#include +#include +#include + +#include +#include +#include +#include + +#include "ignition/gazebo/Entity.hh" + +namespace ignition::gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems::physics_system +{ + // \brief Helper class that associates Gazebo entities with Physics entities + // with required and optional features. It can be used to cast a physics + // entity with the required features to another physics entity with one of + // the optional features. This class was created to keep all physics entities + // in one place so that when a gazebo entity is removed, all the mapped + // physics entitities can be removed at the same time. This ensures that + // reference counts are properly zeroed out in the underlying physics engines + // 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 + // 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 + // overload that also takes an EntityPtr with required features. We can do + // this because there's a 1:1 mapping between the two in maps contained in + // this class. + // + // \tparam PhysicsEntityT Type of entity, such as World, Model, or Link + // \tparam PolicyT Policy of the physics engine (2D, 3D) + // \tparam RequiredFeatureList Required features of the physics entity + // \tparam OptionalFeatureLists One or more optional feature lists of the + // physics entity. + template