diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index dc46afee05..8b177b4b07 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -2,6 +2,12 @@ name: Ubuntu CI on: [push, pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: jammy-ci: runs-on: ubuntu-latest diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml index 6f93ccd58f..2332244bf4 100644 --- a/.github/workflows/triage.yml +++ b/.github/workflows/triage.yml @@ -10,9 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Add ticket to inbox - uses: technote-space/create-project-card-action@v1 + uses: actions/add-to-project@v0.5.0 with: - PROJECT: Core development - COLUMN: Inbox - GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }} - CHECK_ORG_PROJECT: true + project-url: https://github.com/orgs/gazebosim/projects/7 + github-token: ${{ secrets.TRIAGE_TOKEN }} diff --git a/Changelog.md b/Changelog.md index 7f722f8fdd..37462da8b0 100644 --- a/Changelog.md +++ b/Changelog.md @@ -3053,7 +3053,7 @@ 1. Fix generation of systems library symlinks in build directory * [Pull request #1160](https://github.com/gazebosim/gz-sim/pull/1160) -1. Backport sim::Util::validTopic() from gz-sim4. +1. Backport sim::Util::validTopic() from ign-gazebo4. * [Pull request #1153](https://github.com/gazebosim/gz-sim/pull/1153) 1. Support setting the background color for sensors diff --git a/Migration.md b/Migration.md index fe68c5b747..a1df392c5c 100644 --- a/Migration.md +++ b/Migration.md @@ -59,6 +59,11 @@ message's header. * CMake `-config` files * Paths that depend on the project name + * The `Scene3D` plugin has been removed and replaced with `gz-gui`'s `MinimalScene` plugin. See + this same document for the instructions to replace it when it was deprecated 5.x to 6.x. + Setting `false` is no longer required for `TransformControl` and + `ViewAndle` plugins. + * Python library imports such `import ignition.gazebo` and `from ignition import gazebo` should be replaced with `import gz.sim7` and `from gz import sim7`. Note the change from `ignition` to `gz` and the addition of the major version @@ -69,7 +74,6 @@ message's header. + In the Hydrodynamics plugin, inverted the added mass contribution to make it act in the correct direction. - ## Gazebo Sim 6.11.X to 6.12.X * **Modified**: @@ -126,20 +130,23 @@ since pose information is being logged in the `changed_state` topic. * The `GzScene3D` GUI plugin is being deprecated in favor of `MinimalScene`. In order to get the same functionality as `GzScene3D`, users need to add the following plugins: - + `MinimalScene`: base rendering functionality - + `GzSceneManager`: adds / removes / moves entities in the scene - + `EntityContextMenuPlugin`: right-click menu - + `InteractiveViewControl`: orbit controls - + `CameraTracking`: Move to, follow, set camera pose - + `MarkerManager`: Enables the use of markers - + `SelectEntities`: Select entities clicking on the scene - + `Spawn`: Functionality to spawn entities into the scene via GUI - + `VisualizationCapabilities`: View collisions, inertial, CoM, joints, etc. - - Moreover, legacy mode needs to be turned off for the following plugins - for them to work with `MinimalScene` (set `false`): - + `TransformControl`: Translate and rotate - + `ViewAndle`: Move camera to preset angles + + `MinimalScene`: base rendering functionality + + `GzSceneManager`: adds / removes / moves entities in the scene + + `EntityContextMenuPlugin`: right-click menu + + `InteractiveViewControl`: orbit controls + + `CameraTracking`: Move to, follow, set camera pose + + `MarkerManager`: Enables the use of markers + + `SelectEntities`: Select entities clicking on the scene + + `Spawn`: Functionality to spawn entities into the scene via GUI + + `VisualizationCapabilities`: View collisions, inertial, CoM, joints, etc. + + SDF code for all these can be found in: + https://github.com/gazebosim/gz-sim/blob/ff1c82b41e548dfdc8076374f9500db2df2c35a1/examples/worlds/minimal_scene.sdf#L29-L128 + + Moreover, legacy mode needs to be turned off for the following plugins + for them to work with `MinimalScene` (set `false`): + + `TransformControl`: Translate and rotate + + `ViewAndle`: Move camera to preset angles * The `gui.config` and `server.config` files are now located in a versioned folder inside `$HOME/.gz/sim`, i.e. `$HOME/.gz/sim/6/gui.config`. diff --git a/include/gz/sim/InstallationDirectories.hh b/include/gz/sim/InstallationDirectories.hh new file mode 100644 index 0000000000..08e3b2b29e --- /dev/null +++ b/include/gz/sim/InstallationDirectories.hh @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2023 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 GZ_SIM_INSTALLATION_DIRECTORIES_HH_ +#define GZ_SIM_INSTALLATION_DIRECTORIES_HH_ + +#include + +#include +#include + +namespace gz +{ + namespace sim + { + inline namespace GZ_SIM_VERSION_NAMESPACE { + + /// \brief getInstallPrefix return the install prefix of the library + /// i.e. CMAKE_INSTALL_PREFIX unless the library has been moved + GZ_SIM_VISIBLE std::string getInstallPrefix(); + + /// \brief getGUIConfigPath return the GUI config path + GZ_SIM_VISIBLE std::string getGUIConfigPath(); + + /// \brief getSystemConfigPath return the system config path + GZ_SIM_VISIBLE std::string getSystemConfigPath(); + + /// \brief getServerConfigPath return the server config path + GZ_SIM_VISIBLE std::string getServerConfigPath(); + + /// \brief getPluginInstallDir return the plugin install dir + GZ_SIM_VISIBLE std::string getPluginInstallDir(); + + /// \brief getGUIPluginInstallDir return the GUI plugin install dir + GZ_SIM_VISIBLE std::string getGUIPluginInstallDir(); + + /// \brief getWorldInstallDir return the world install dir + GZ_SIM_VISIBLE std::string getWorldInstallDir(); + } + } +} + +#endif diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index f54f432630..d67a17d4de 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -306,6 +306,19 @@ namespace gz const math::Vector3d &_force, const math::Vector3d &_torque) const; + /// \brief Add a wrench expressed in world coordinates and applied to + /// the link at an offset from the link's origin. This wrench + /// is applied for one simulation step. + /// \param[in] _ecm Mutable Entity-component manager. + /// \param[in] _force Force to be applied expressed in world coordinates + /// \param[in] _torque Torque to be applied expressed in world coordinates + /// \param[in] _offset The point of application of the force expressed + /// in the link frame + public: void AddWorldWrench(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_torque, + const math::Vector3d &_offset) const; + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/include/gz/sim/ServerConfig.hh b/include/gz/sim/ServerConfig.hh index a40258807f..bfb0e3e362 100644 --- a/include/gz/sim/ServerConfig.hh +++ b/include/gz/sim/ServerConfig.hh @@ -324,14 +324,14 @@ namespace gz UpdatePeriod() const; /// \brief Path to where simulation resources, such as models downloaded - /// from fuel.ignitionrobotics.org, should be stored. + /// from fuel.gazebosim.org, should be stored. /// \return Path to a location on disk. An empty string indicates that /// the default value will be used, which is currently /// ~/.gz/fuel. public: const std::string &ResourceCache() const; /// \brief Set the path to where simulation resources, such as models - /// downloaded from fuel.ignitionrobotics.org, should be stored. + /// downloaded from fuel.gazebosim.org, should be stored. /// \param[in] _path Path to a location on disk. An empty string /// indicates that the default value will be used, which is currently /// ~/.gz/fuel. diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index 8e84199ddf..e918cb7fb8 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -24,7 +24,9 @@ #include #include +#include #include +#include #include "gz/sim/components/Environment.hh" #include "gz/sim/config.hh" @@ -309,6 +311,11 @@ namespace gz const math::Vector3d& _worldPosition, const std::shared_ptr& _gridField); + /// \brief Load a mesh from a Mesh SDF DOM + /// \param[in] _meshSdf Mesh SDF DOM + /// \return The loaded mesh or null if the mesh can not be loaded. + GZ_SIM_VISIBLE const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"}; diff --git a/include/gz/sim/config.hh.in b/include/gz/sim/config.hh.in index 894bd0d754..a13b62aa1e 100644 --- a/include/gz/sim/config.hh.in +++ b/include/gz/sim/config.hh.in @@ -33,12 +33,12 @@ #define GZ_SIM_VERSION_HEADER "Gazebo Sim, version ${PROJECT_VERSION_FULL}\nCopyright (C) 2018 Open Source Robotics Foundation.\nReleased under the Apache 2.0 License.\n\n" -#define GZ_SIM_GUI_CONFIG_PATH "${CMAKE_INSTALL_PREFIX}/${GZ_DATA_INSTALL_DIR}/gui" -#define GZ_SIM_SYSTEM_CONFIG_PATH "${CMAKE_INSTALL_PREFIX}/${GZ_DATA_INSTALL_DIR}/systems" -#define GZ_SIM_SERVER_CONFIG_PATH "${CMAKE_INSTALL_PREFIX}/${GZ_DATA_INSTALL_DIR}" -#define GZ_SIM_PLUGIN_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins" -#define GZ_SIM_GUI_PLUGIN_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins/gui" -#define GZ_SIM_WORLD_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/${GZ_DATA_INSTALL_DIR}/worlds" +#define GZ_SIM_GUI_CONFIG_PATH _Pragma ("GCC warning \"'GZ_SIM_GUI_CONFIG_PATH' macro is deprecated, use gz::sim::getGUIConfigPath() function instead. \"") "${CMAKE_INSTALL_PREFIX}/${GZ_DATA_INSTALL_DIR}/gui" +#define GZ_SIM_SYSTEM_CONFIG_PATH _Pragma ("GCC warning \"'GZ_SIM_SYSTEM_CONFIG_PATH' macro is deprecated, use gz::sim::getSystemConfigPath() function instead. \"") "${CMAKE_INSTALL_PREFIX}/${GZ_DATA_INSTALL_DIR}/systems" +#define GZ_SIM_SERVER_CONFIG_PATH _Pragma ("GCC warning \"'GZ_SIM_SERVER_CONFIG_PATH' macro is deprecated, use gz::sim::getServerConfigPath() function instead. \"") "${CMAKE_INSTALL_PREFIX}/${GZ_DATA_INSTALL_DIR}" +#define GZ_SIM_PLUGIN_INSTALL_DIR _Pragma ("GCC warning \"'GZ_SIM_PLUGIN_INSTALL_DIR' macro is deprecated, use gz::sim::getPluginInstallDir() function instead. \"") "${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins" +#define GZ_SIM_GUI_PLUGIN_INSTALL_DIR _Pragma ("GCC warning \"'GZ_SIM_GUI_PLUGIN_INSTALL_DIR' macro is deprecated, use gz::sim::getGUIPluginInstallDir() function instead. \"") "${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins/gui" +#define GZ_SIM_WORLD_INSTALL_DIR _Pragma ("GCC warning \"'GZ_SIM_WORLD_INSTALL_DIR' macro is deprecated, use gz::sim::getWorldInstallDir() function instead. \"") "${CMAKE_INSTALL_PREFIX}/${GZ_DATA_INSTALL_DIR}/worlds" #define GZ_DISTRIBUTION "${GZ_DISTRIBUTION}" #endif diff --git a/include/gz/sim/rendering/MarkerManager.hh b/include/gz/sim/rendering/MarkerManager.hh index 3bed374c0d..68492b6f9e 100644 --- a/include/gz/sim/rendering/MarkerManager.hh +++ b/include/gz/sim/rendering/MarkerManager.hh @@ -20,6 +20,7 @@ #include #include +#include #include #include "gz/rendering/RenderTypes.hh" diff --git a/include/gz/sim/rendering/WrenchVisualizer.hh b/include/gz/sim/rendering/WrenchVisualizer.hh new file mode 100644 index 0000000000..c0a382467e --- /dev/null +++ b/include/gz/sim/rendering/WrenchVisualizer.hh @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2023 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 GZ_SIM_WRENCHVISUALIZER_HH_ +#define GZ_SIM_WRENCHVISUALIZER_HH_ + +#include + +#include +#include +#include + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace detail +{ + /// \brief Creates, deletes, and maintains force and torque visuals + class GZ_SIM_RENDERING_VISIBLE WrenchVisualizer + { + /// \brief Constructor + public: WrenchVisualizer(); + + /// \brief Destructor + public: ~WrenchVisualizer(); + + /// \brief Initialize the Wrench visualizer + /// \param[in] _scene The rendering scene where the visuals are created + /// \return True if the scene is valid + bool Init(rendering::ScenePtr _scene); + + /// \brief Create a new force visual + /// \param[in] _material The material used for the visual + /// \return Pointer to the created ArrowVisual + public: rendering::ArrowVisualPtr CreateForceVisual( + rendering::MaterialPtr _material); + + /// \brief Create a new torque visual + /// \param[in] _material The material used for the visual + /// \return Pointer to the created Visual + public: rendering::VisualPtr CreateTorqueVisual( + rendering::MaterialPtr _material); + + /// \brief Update the visual of a vector to match its direction and position + /// \param[in] _visual Pointer to the vector visual to be updated + /// \param[in] _direction Direction of the vector + /// \param[in] _position Position of the arrow + /// \param[in] _size Size of the arrow in meters + /// \param[in] _tip True if _position specifies the tip of the vector, + /// false if it specifies tha base of the vector + public: void UpdateVectorVisual(rendering::VisualPtr _visual, + const math::Vector3d &_direction, + const math::Vector3d &_position, + const double _size, + const bool _tip = false); + + /// \internal + /// \brief Private data pointer + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; +} +} +} +} +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5f07404905..ed63da4cf9 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -50,6 +50,7 @@ set (sources ComponentFactory.cc EntityComponentManager.cc EntityComponentManagerDiff.cc + InstallationDirectories.cc Joint.cc LevelManager.cc Light.cc @@ -163,6 +164,21 @@ target_link_libraries(${gz_lib_target} # Create the library target gz_create_core_library(SOURCES ${sources} CXX_STANDARD 17) +gz_add_get_install_prefix_impl(GET_INSTALL_PREFIX_FUNCTION gz::sim::getInstallPrefix + GET_INSTALL_PREFIX_HEADER gz/sim/InstallationDirectories.hh + OVERRIDE_INSTALL_PREFIX_ENV_VARIABLE GZ_SIM_INSTALL_PREFIX) + +set_property( + SOURCE InstallationDirectories.cc + PROPERTY COMPILE_DEFINITIONS + GZ_SIM_GUI_CONFIG_RELATIVE_PATH="${GZ_DATA_INSTALL_DIR}/gui" + GZ_SIM_SYSTEM_CONFIG_RELATIVE_PATH="${GZ_DATA_INSTALL_DIR}/systems" + GZ_SIM_SERVER_CONFIG_RELATIVE_PATH="${GZ_DATA_INSTALL_DIR}" + GZ_SIM_PLUGIN_RELATIVE_INSTALL_DIR="${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins" + GZ_SIM_GUI_PLUGIN_RELATIVE_INSTALL_DIR="${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins/gui" + GZ_SIM_WORLD_RELATIVE_INSTALL_DIR="${GZ_DATA_INSTALL_DIR}/worlds" +) + target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC gz-math${GZ_MATH_VER} @@ -208,6 +224,8 @@ gz_build_tests(TYPE UNIT ${PROJECT_LIBRARY_TARGET_NAME} ${EXTRA_TEST_LIB_DEPS} gz-sim${PROJECT_VERSION_MAJOR} + ENVIRONMENT + GZ_SIM_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) # Some server unit tests require rendering. diff --git a/src/InstallationDirectories.cc b/src/InstallationDirectories.cc new file mode 100644 index 0000000000..2aa64e334d --- /dev/null +++ b/src/InstallationDirectories.cc @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2023 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. + * +*/ + +#include +#include + +#include + +namespace gz +{ +namespace sim +{ +inline namespace GZ_SIM_VERSION_NAMESPACE { + +std::string getGUIConfigPath() +{ + return gz::common::joinPaths( + getInstallPrefix(), GZ_SIM_GUI_CONFIG_RELATIVE_PATH); +} + +std::string getSystemConfigPath() +{ + return gz::common::joinPaths( + getInstallPrefix(), GZ_SIM_SYSTEM_CONFIG_RELATIVE_PATH); +} + +std::string getServerConfigPath() +{ + return gz::common::joinPaths( + getInstallPrefix(), GZ_SIM_SERVER_CONFIG_RELATIVE_PATH); +} + +std::string getPluginInstallDir() +{ + return gz::common::joinPaths( + getInstallPrefix(), GZ_SIM_PLUGIN_RELATIVE_INSTALL_DIR); +} + +std::string getGUIPluginInstallDir() +{ + return gz::common::joinPaths( + getInstallPrefix(), GZ_SIM_GUI_PLUGIN_RELATIVE_INSTALL_DIR); +} + +std::string getWorldInstallDir() +{ + return gz::common::joinPaths( + getInstallPrefix(), GZ_SIM_WORLD_RELATIVE_INSTALL_DIR); +} + +} +} +} diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 9d35fae62a..0513fb4f30 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -38,14 +38,14 @@ #include "gz/sim/components/Gravity.hh" #include "gz/sim/components/Joint.hh" #include "gz/sim/components/Level.hh" -#include "gz/sim/components/Model.hh" -#include "gz/sim/components/Light.hh" -#include "gz/sim/components/Name.hh" #include "gz/sim/components/LevelBuffer.hh" #include "gz/sim/components/LevelEntityNames.hh" +#include "gz/sim/components/Light.hh" #include "gz/sim/components/LinearVelocity.hh" #include "gz/sim/components/LinearVelocitySeed.hh" #include "gz/sim/components/MagneticField.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Performer.hh" #include "gz/sim/components/PerformerLevels.hh" diff --git a/src/Link.cc b/src/Link.cc index bf58837f29..8378661dbe 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -412,23 +412,46 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, const math::Vector3d &_force, const math::Vector3d &_torque) const { - auto linkWrenchComp = - _ecm.Component(this->dataPtr->id); + this->AddWorldWrench(_ecm, _force, _torque, math::Vector3d::Zero); +} + +////////////////////////////////////////////////// +void Link::AddWorldWrench(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_torque, + const math::Vector3d &_offset) const +{ + math::Pose3d linkWorldPose; + auto worldPoseComp = _ecm.Component(this->dataPtr->id); + if (worldPoseComp) + { + linkWorldPose = worldPoseComp->Data(); + } + else + { + linkWorldPose = worldPose(this->dataPtr->id, _ecm); + } - components::ExternalWorldWrenchCmd wrench; + // We want the force to be applied at an offset from the link origin, so we + // must compute the resulting force and torque on the link origin. + auto posComWorldCoord = linkWorldPose.Rot().RotateVector(_offset); + math::Vector3d torqueWithOffset = _torque + posComWorldCoord.Cross(_force); + auto linkWrenchComp = + _ecm.Component(this->dataPtr->id); if (!linkWrenchComp) { + components::ExternalWorldWrenchCmd wrench; msgs::Set(wrench.Data().mutable_force(), _force); - msgs::Set(wrench.Data().mutable_torque(), _torque); + msgs::Set(wrench.Data().mutable_torque(), torqueWithOffset); _ecm.CreateComponent(this->dataPtr->id, wrench); } else { msgs::Set(linkWrenchComp->Data().mutable_force(), - msgs::Convert(linkWrenchComp->Data().force()) + _force); + msgs::Convert(linkWrenchComp->Data().force()) + _force); msgs::Set(linkWrenchComp->Data().mutable_torque(), - msgs::Convert(linkWrenchComp->Data().torque()) + _torque); + msgs::Convert(linkWrenchComp->Data().torque()) + torqueWithOffset); } } diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index bfb18b8d5d..97a99ae791 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -25,6 +25,7 @@ #include #include +#include "gz/sim/InstallationDirectories.hh" #include "gz/sim/Util.hh" using namespace gz; @@ -206,7 +207,8 @@ class gz::sim::ServerConfigPrivate networkSecondaries(_cfg->networkSecondaries), seed(_cfg->seed), logRecordTopics(_cfg->logRecordTopics), - isHeadlessRendering(_cfg->isHeadlessRendering) { } + isHeadlessRendering(_cfg->isHeadlessRendering), + source(_cfg->source){ } // \brief The SDF file that the server should load public: std::string sdfFile = ""; @@ -785,6 +787,7 @@ ServerConfig::SourceType ServerConfig::Source() const } ///////////////////////////////////////////////// +namespace { void copyElement(sdf::ElementPtr _sdf, const tinyxml2::XMLElement *_xml) { _sdf->SetName(_xml->Value()); @@ -891,6 +894,7 @@ parsePluginsFromDoc(const tinyxml2::XMLDocument &_doc) } return ret; } +} // namespace ///////////////////////////////////////////////// std::list @@ -987,7 +991,7 @@ sim::loadPluginInfo(bool _isPlayback) if (!common::exists(defaultConfig)) { auto installedConfig = common::joinPaths( - GZ_SIM_SERVER_CONFIG_PATH, + gz::sim::getServerConfigPath(), configFilename); if (!common::createDirectories(defaultConfigDir)) diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index 47cfccf61a..1807819674 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -32,6 +32,7 @@ #include +#include "gz/sim/InstallationDirectories.hh" #include using namespace gz::sim; @@ -54,7 +55,7 @@ class gz::sim::SystemLoaderPrivate common::env(GZ_HOMEDIR, homePath); systemPaths.AddPluginPaths(common::joinPaths( homePath, ".gz", "sim", "plugins")); - systemPaths.AddPluginPaths(GZ_SIM_PLUGIN_INSTALL_DIR); + systemPaths.AddPluginPaths(gz::sim::getPluginInstallDir()); return systemPaths.PluginPaths(); } diff --git a/src/SystemLoader_TEST.cc b/src/SystemLoader_TEST.cc index b654924f34..4709af5fd8 100644 --- a/src/SystemLoader_TEST.cc +++ b/src/SystemLoader_TEST.cc @@ -97,9 +97,8 @@ TEST(SystemLoader, PluginPaths) for (const auto &s : paths) { // the returned path string may not be exact match due to extra '/' - // appended at the end of the string. So use absPath to generate - // a path string that matches the format returned by joinPaths - if (common::absPath(s) == testBuildPath) + // appended at the end of the string. So use absPath to compare paths + if (common::absPath(s) == common::absPath(testBuildPath)) { hasPath = true; break; diff --git a/src/Util.cc b/src/Util.cc index b8d658cf96..85e21e4e4f 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -18,7 +18,10 @@ #include #include +#include +#include #include +#include #include #include #include @@ -49,6 +52,7 @@ #include "gz/sim/components/World.hh" #include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/InstallationDirectories.hh" #include "gz/sim/Util.hh" namespace gz @@ -800,13 +804,51 @@ std::string resolveSdfWorldFile(const std::string &_sdfFile, systemPaths.SetFilePathEnv(kResourcePathEnv); // Worlds installed with gz-sim - systemPaths.AddFilePaths(GZ_SIM_WORLD_INSTALL_DIR); + systemPaths.AddFilePaths(gz::sim::getWorldInstallDir()); filePath = systemPaths.FindFile(_sdfFile); } return filePath; } + +const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf) +{ + const common::Mesh *mesh = nullptr; + auto &meshManager = *common::MeshManager::Instance(); + if (common::URI(_meshSdf.Uri()).Scheme() == "name") + { + // if it has a name:// scheme, see if the mesh + // exists in the mesh manager and load it by name + const std::string basename = common::basename(_meshSdf.Uri()); + mesh = meshManager.MeshByName(basename); + if (nullptr == mesh) + { + gzwarn << "Failed to load mesh by name [" << basename + << "]." << std::endl; + return nullptr; + } + } + else if (meshManager.IsValidFilename(_meshSdf.Uri())) + { + // load mesh by file path + auto fullPath = asFullPath(_meshSdf.Uri(), _meshSdf.FilePath()); + mesh = meshManager.Load(fullPath); + if (nullptr == mesh) + { + gzwarn << "Failed to load mesh from [" << fullPath + << "]." << std::endl; + return nullptr; + } + } + else + { + gzwarn << "Failed to load mesh [" << _meshSdf.Uri() + << "]." << std::endl; + return nullptr; + } + return mesh; +} } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 9b291983f6..889234a9d0 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -40,6 +40,7 @@ #include "gz/sim/Util.hh" #include "helpers/EnvTestFixture.hh" +#include "test_config.hh" using namespace gz; using namespace sim; @@ -1003,3 +1004,23 @@ TEST_F(UtilTest, ResolveSdfWorldFile) // A bad relative path should return an empty string EXPECT_TRUE(resolveSdfWorldFile("../invalid/does_not_exist.sdf").empty()); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, LoadMesh) +{ + sdf::Mesh meshSdf; + EXPECT_EQ(nullptr, loadMesh(meshSdf)); + + meshSdf.SetUri("invalid_uri"); + meshSdf.SetFilePath("invalid_filepath"); + EXPECT_EQ(nullptr, loadMesh(meshSdf)); + + meshSdf.SetUri("name://unit_box"); + EXPECT_NE(nullptr, loadMesh(meshSdf)); + + meshSdf.SetUri("duck.dae"); + std::string filePath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "media", "duck.dae"); + meshSdf.SetFilePath(filePath); + EXPECT_NE(nullptr, loadMesh(meshSdf)); +} diff --git a/src/gui/CMakeLists.txt b/src/gui/CMakeLists.txt index 0f774b50f2..472eba232d 100644 --- a/src/gui/CMakeLists.txt +++ b/src/gui/CMakeLists.txt @@ -75,6 +75,8 @@ gz_build_tests(TYPE UNIT gz-sim${PROJECT_VERSION_MAJOR} gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER} + ENVIRONMENT + GZ_SIM_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) if(TARGET UNIT_Gui_clean_exit_TEST) diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 7b1e836c33..72c6335e4d 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -30,6 +30,7 @@ #include #include +#include "gz/sim/InstallationDirectories.hh" #include "gz/sim/Util.hh" #include "gz/sim/config.hh" #include "gz/sim/gui/Gui.hh" @@ -93,7 +94,7 @@ std::string defaultGuiConfigFile(bool _isPlayback, } auto installedConfig = common::joinPaths( - GZ_SIM_GUI_CONFIG_PATH, defaultGuiConfigName); + gz::sim::getGUIConfigPath(), defaultGuiConfigName); if (!common::copyFile(installedConfig, defaultConfig)) { gzerr << "Failed to copy installed config [" << installedConfig @@ -277,7 +278,7 @@ std::unique_ptr createGui( auto app = std::make_unique( _argc, _argv, gz::gui::WindowType::kMainWindow, _renderEngineGuiApiBackend); - app->AddPluginPath(GZ_SIM_GUI_PLUGIN_INSTALL_DIR); + app->AddPluginPath(gz::sim::getGUIPluginInstallDir()); auto aboutDialogHandler = new gz::sim::gui::AboutDialogHandler(); aboutDialogHandler->setParent(app->Engine()); @@ -289,7 +290,7 @@ std::unique_ptr createGui( pathManager->setParent(app->Engine()); // add import path so we can load custom modules - app->Engine()->addImportPath(GZ_SIM_GUI_PLUGIN_INSTALL_DIR); + app->Engine()->addImportPath(gz::sim::getGUIPluginInstallDir().c_str()); app->SetDefaultConfigPath(defaultConfig); @@ -440,8 +441,10 @@ std::unique_ptr createGui( "VisualizationCapabilities"}; std::string msg{ - "The [GzScene3D] GUI plugin has been removed since Garden. " - "Loading the following plugins instead:\n"}; + "The [GzScene3D] GUI plugin has been removed since Garden.\n" + "SDF code to replace GzScene3D is available at " + "https://github.com/gazebosim/gz-sim/blob/gz-sim7/Migration.md\n" + "Loading the following plugins instead:\n"}; for (auto extra : extras) { diff --git a/src/gui/QuickStartHandler.cc b/src/gui/QuickStartHandler.cc index c420bffc64..8659b2756d 100644 --- a/src/gui/QuickStartHandler.cc +++ b/src/gui/QuickStartHandler.cc @@ -17,13 +17,16 @@ #include "QuickStartHandler.hh" +#include "gz/sim/InstallationDirectories.hh" + using namespace gz; using namespace sim::gui; ///////////////////////////////////////////////// QString QuickStartHandler::WorldsPath() const { - return QString::fromUtf8(this->worldsPath.c_str()); + std::string worldsPathLocal = gz::sim::getWorldInstallDir(); + return QString::fromUtf8(worldsPathLocal.c_str()); } ///////////////////////////////////////////////// diff --git a/src/gui/QuickStartHandler.hh b/src/gui/QuickStartHandler.hh index e4648b5bb8..5814adce08 100644 --- a/src/gui/QuickStartHandler.hh +++ b/src/gui/QuickStartHandler.hh @@ -68,7 +68,9 @@ class GZ_SIM_GUI_VISIBLE QuickStartHandler : public QObject private: bool showAgain{true}; /// \brief Installed worlds path. - private: std::string worldsPath{GZ_SIM_WORLD_INSTALL_DIR}; + /// \warning This variable is unused and can be removed + /// once an ABI break is allowed + private: std::string worldsPath{""}; /// \brief Get starting world url. private: std::string startingWorld{""}; diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index ec0644a287..93cf416713 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -118,7 +118,9 @@ function(gz_add_gui_plugin plugin_name) # Used to make test-directory headers visible to the unit tests ${PROJECT_SOURCE_DIR} # Used to make test_config.h visible to the unit tests - ${PROJECT_BINARY_DIR}) + ${PROJECT_BINARY_DIR} + ENVIRONMENT + GZ_SIM_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) endif() endif() @@ -129,6 +131,7 @@ add_subdirectory(modules) # Plugins add_subdirectory(align_tool) +add_subdirectory(apply_force_torque) add_subdirectory(banana_for_scale) add_subdirectory(component_inspector) add_subdirectory(component_inspector_editor) @@ -141,6 +144,7 @@ add_subdirectory(global_illumination_civct) add_subdirectory(global_illumination_vct) add_subdirectory(joint_position_controller) add_subdirectory(lights) +add_subdirectory(mouse_drag) add_subdirectory(playback_scrubber) add_subdirectory(plot_3d) add_subdirectory(plotting) diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc new file mode 100644 index 0000000000..6d42ddff22 --- /dev/null +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc @@ -0,0 +1,915 @@ +/* + * Copyright (C) 2023 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. + * +*/ + +#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 "ApplyForceTorque.hh" + +namespace gz +{ +namespace sim +{ + /// \enum RotationToolVector + /// \brief Unique identifiers for which vector is currently being + /// modified by the rotation tool + enum RotationToolVector + { + /// \brief Rotation tool is inactive + NONE = 0, + /// \brief Force vector + FORCE = 1, + /// \brief Torque vector + TORQUE = 2, + }; + + class ApplyForceTorquePrivate + { + /// \brief Publish EntityWrench messages in order to apply force and torque + /// \param[in] _applyForce True if the force should be applied + /// \param[in] _applyTorque True if the torque should be applied + public: void PublishWrench(bool _applyForce, bool _applyTorque); + + /// \brief Perform rendering calls in the rendering thread. + public: void OnRender(); + + /// \brief Update visuals for force and torque + public: void UpdateVisuals(); + + /// \brief Handle mouse events + public: void HandleMouseEvents(); + + /// \brief Transport node + public: transport::Node node; + + /// \brief Publisher for EntityWrench messages + public: transport::Node::Publisher pub; + + /// \brief World name + public: std::string worldName; + + /// \brief True if the ApplyLinkWrench system is loaded + public: bool systemLoaded{false}; + + /// \brief To synchronize member access + public: std::mutex mutex; + + /// \brief Name of the selected model + public: QString modelName; + + /// \brief List of the name of the links in the selected model + public: QStringList linkNameList; + + /// \brief Index of selected link in list + public: int linkIndex{-1}; + + /// \brief Entity of the currently selected Link + public: std::optional selectedEntity; + + /// \brief True if a new entity was selected + public: bool changedEntity{false}; + + /// \brief True if a new link was selected from the dropdown + public: bool changedIndex{false}; + + /// \brief Force to be applied in link-fixed frame + public: math::Vector3d force{0.0, 0.0, 0.0}; + + /// \brief Offset of force application point in link-fixed frame + /// relative to the center of mass + public: math::Vector3d offset{0.0, 0.0, 0.0}; + + /// \brief Torque to be applied in link-fixed frame + public: math::Vector3d torque{0.0, 0.0, 0.0}; + + /// \brief Offset from the link origin to the center of mass in world coords + public: math::Vector3d inertialPos; + + /// \brief Pose of the link-fixed frame + public: math::Pose3d linkWorldPose; + + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + + /// \brief Ray used for checking intersection with planes for computing + /// 3d world coordinates from 2d + public: rendering::RayQueryPtr ray{nullptr}; + + /// \brief True if there are new mouse events to process. + public: bool mouseDirty{false}; + + /// \brief True if the rotation tool modified the force or torque vector + public: bool vectorDirty{false}; + + /// \brief Whether the transform gizmo is being dragged. + public: bool transformActive{false}; + + /// \brief Block orbit + public: bool blockOrbit{false}; + + /// \brief True if BlockOrbit events should be sent + public: bool sendBlockOrbit{false}; + + /// \brief Where the mouse left off + public: math::Vector2i mousePressPos = math::Vector2i::Zero; + + /// \brief Holds the latest mouse event + public: gz::common::MouseEvent mouseEvent; + + /// \brief Vector (force or torque) currently being rotated + /// by the rotation tool + public: RotationToolVector activeVector{RotationToolVector::NONE}; + + /// \brief Vector value on start of rotation tool transformation, + /// relative to link + public: math::Vector3d initialVector; + + /// \brief Vector orientation on start of rotation tool transformation, + /// relative to link + public: math::Quaterniond initialVectorRot; + + /// \brief Current orientation of the transformed vector relative to link + public: math::Quaterniond vectorRot; + + /// \brief Active transformation axis on the rotation tool + public: rendering::TransformAxis activeAxis{rendering::TA_NONE}; + + /// \brief Wrench visualizer + public: detail::WrenchVisualizer wrenchVis; + + /// \brief Arrow for visualizing force. + public: rendering::ArrowVisualPtr forceVisual{nullptr}; + + /// \brief Arrow for visualizing torque. + public: rendering::VisualPtr torqueVisual{nullptr}; + + /// \brief Gizmo visual for rotating vectors in rotation tool + public: rendering::GizmoVisualPtr gizmoVisual{nullptr}; + }; +} +} + +using namespace gz; +using namespace sim; + +///////////////////////////////////////////////// +ApplyForceTorque::ApplyForceTorque() + : GuiSystem(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +ApplyForceTorque::~ApplyForceTorque() = default; + +///////////////////////////////////////////////// +void ApplyForceTorque::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) +{ + if (this->title.empty()) + this->title = "Apply force and torque"; + + // Create wrench publisher + auto worldNames = gz::gui::worldNames(); + if (!worldNames.empty()) + { + this->dataPtr->worldName = worldNames[0].toStdString(); + auto topic = transport::TopicUtils::AsValidTopic( + "/world/" + this->dataPtr->worldName + "/wrench"); + if (topic == "") + { + gzerr << "Unable to create publisher" << std::endl; + return; + } + this->dataPtr->pub = + this->dataPtr->node.Advertise(topic); + gzdbg << "Created publisher to " << topic << std::endl; + } + + gz::gui::App()->findChild + ()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool ApplyForceTorque::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gz::gui::events::Render::kType) + { + this->dataPtr->OnRender(); + + if (this->dataPtr->vectorDirty) + { + this->dataPtr->vectorDirty = false; + if (this->dataPtr->activeVector == RotationToolVector::FORCE) + emit this->ForceChanged(); + else if (this->dataPtr->activeVector == RotationToolVector::TORQUE) + emit this->TorqueChanged(); + } + } + else if (_event->type() == gz::sim::gui::events::EntitiesSelected::kType) + { + if (!this->dataPtr->blockOrbit && !this->dataPtr->mouseEvent.Dragging()) + { + gz::sim::gui::events::EntitiesSelected *_e = + static_cast(_event); + this->dataPtr->selectedEntity = _e->Data().front(); + this->dataPtr->changedEntity = true; + } + } + else if (_event->type() == gz::sim::gui::events::DeselectAllEntities::kType) + { + if (!this->dataPtr->blockOrbit && !this->dataPtr->mouseEvent.Dragging()) + { + this->dataPtr->selectedEntity.reset(); + this->dataPtr->changedEntity = true; + } + } + else if (_event->type() == gz::gui::events::LeftClickOnScene::kType) + { + gz::gui::events::LeftClickOnScene *_e = + static_cast(_event); + this->dataPtr->mouseEvent = _e->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::MousePressOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::DragOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::Update(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + std::lock_guard lock(this->dataPtr->mutex); + + // Load the ApplyLinkWrench system + // TODO(anyone): should this be checked on every Update instead? + if (!this->dataPtr->systemLoaded) + { + const std::string name{"gz::sim::systems::ApplyLinkWrench"}; + const std::string filename{"gz-sim-apply-link-wrench-system"}; + const std::string innerxml{"0"}; + + // Get world entity + Entity worldEntity; + _ecm.Each( + [&](const Entity &_entity, + const components::World */*_world*/, + const components::Name *_name)->bool + { + if (_name->Data() == this->dataPtr->worldName) + { + worldEntity = _entity; + return false; + } + return true; + }); + + // Check if already loaded + auto msg = _ecm.ComponentData(worldEntity); + if (!msg) + { + gzdbg << "Unable to find SystemPluginInfo component for entity " + << worldEntity << std::endl; + return; + } + for (const auto &plugin : msg->plugins()) + { + if (plugin.filename() == filename) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system already loaded" << std::endl; + break; + } + } + + // Request to load system + if (!this->dataPtr->systemLoaded) + { + msgs::EntityPlugin_V req; + req.mutable_entity()->set_id(worldEntity); + auto plugin = req.add_plugins(); + plugin->set_name(name); + plugin->set_filename(filename); + plugin->set_innerxml(innerxml); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/" + this->dataPtr->worldName + + "/entity/system/add"}; + if (this->dataPtr->node.Request(service, req, timeout, res, result)) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system has been loaded" << std::endl; + } + else + { + gzerr << "Error adding new system to entity: " + << worldEntity << "\n" + << "Name: " << name << "\n" + << "Filename: " << filename << "\n" + << "Inner XML: " << innerxml << std::endl; + } + } + } + + if (this->dataPtr->changedEntity) + { + this->dataPtr->changedEntity = false; + + this->dataPtr->modelName = ""; + this->dataPtr->linkNameList.clear(); + this->dataPtr->linkIndex = -1; + + if (this->dataPtr->selectedEntity.has_value()) + { + Model parentModel(*this->dataPtr->selectedEntity); + Link selectedLink(*this->dataPtr->selectedEntity); + if (parentModel.Valid(_ecm)) + { + selectedLink = Link(parentModel.CanonicalLink(_ecm)); + } + else if (selectedLink.Valid(_ecm)) + { + parentModel = *selectedLink.ParentModel(_ecm); + } + else + { + this->dataPtr->selectedEntity.reset(); + return; + } + + this->dataPtr->modelName = QString::fromStdString( + parentModel.Name(_ecm)); + this->dataPtr->selectedEntity = selectedLink.Entity(); + + // Put all of the model's links into the list + auto links = parentModel.Links(_ecm); + unsigned int i{0}; + while (i < links.size()) + { + Link link(links[i]); + this->dataPtr->linkNameList.push_back( + QString::fromStdString(*link.Name(_ecm))); + if (link.Entity() == this->dataPtr->selectedEntity) + { + this->dataPtr->linkIndex = i; + } + ++i; + } + } + } + + if (this->dataPtr->changedIndex) + { + this->dataPtr->changedIndex = false; + + if (this->dataPtr->selectedEntity.has_value()) + { + auto parentModel = Link(*this->dataPtr->selectedEntity).ParentModel(_ecm); + std::string linkName = + this->dataPtr->linkNameList[this->dataPtr->linkIndex].toStdString(); + this->dataPtr->selectedEntity = parentModel->LinkByName(_ecm, linkName); + } + } + + // Get the position of the center of mass and link orientation + if (this->dataPtr->selectedEntity.has_value()) + { + auto linkWorldPose = worldPose(*this->dataPtr->selectedEntity, _ecm); + auto inertial = _ecm.Component( + *this->dataPtr->selectedEntity); + if (inertial) + { + this->dataPtr->inertialPos = + linkWorldPose.Rot().RotateVector(inertial->Data().Pose().Pos()); + this->dataPtr->linkWorldPose = linkWorldPose; + } + } + + emit this->ModelNameChanged(); + emit this->LinkNameListChanged(); + emit this->LinkIndexChanged(); +} + +///////////////////////////////////////////////// +QString ApplyForceTorque::ModelName() const +{ + return this->dataPtr->modelName; +} + +///////////////////////////////////////////////// +QStringList ApplyForceTorque::LinkNameList() const +{ + return this->dataPtr->linkNameList; +} + +///////////////////////////////////////////////// +int ApplyForceTorque::LinkIndex() const +{ + return this->dataPtr->linkIndex; +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetLinkIndex(int _linkIndex) +{ + this->dataPtr->linkIndex = _linkIndex; + this->dataPtr->changedIndex = true; +} + +///////////////////////////////////////////////// +QVector3D ApplyForceTorque::Force() const +{ + return QVector3D( + this->dataPtr->force.X(), + this->dataPtr->force.Y(), + this->dataPtr->force.Z()); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetForce(QVector3D _force) +{ + this->dataPtr->force.Set(_force.x(), _force.y(), _force.z()); + // Update rotation tool orientation when force is set by components + if (this->dataPtr->activeVector == RotationToolVector::FORCE + && this->dataPtr->activeAxis == rendering::TransformAxis::TA_NONE) + { + this->dataPtr->vectorRot = math::Matrix4d::LookAt( + -this->dataPtr->force, math::Vector3d::Zero).Rotation(); + } +} + +///////////////////////////////////////////////// +QVector3D ApplyForceTorque::Torque() const +{ + return QVector3D( + this->dataPtr->torque.X(), + this->dataPtr->torque.Y(), + this->dataPtr->torque.Z()); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetTorque(QVector3D _torque) +{ + this->dataPtr->torque.Set(_torque.x(), _torque.y(), _torque.z()); + // Update rotation tool orientation when torque is set by components + if (this->dataPtr->activeVector == RotationToolVector::TORQUE + && this->dataPtr->activeAxis == rendering::TransformAxis::TA_NONE) + { + this->dataPtr->vectorRot = math::Matrix4d::LookAt( + -this->dataPtr->torque, math::Vector3d::Zero).Rotation(); + } +} + +///////////////////////////////////////////////// +void ApplyForceTorque::ApplyForce() +{ + this->dataPtr->PublishWrench(true, false); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::ApplyTorque() +{ + this->dataPtr->PublishWrench(false, true); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::ApplyAll() +{ + this->dataPtr->PublishWrench(true, true); +} + +///////////////////////////////////////////////// +void ApplyForceTorquePrivate::PublishWrench(bool _applyForce, bool _applyTorque) +{ + std::lock_guard lock(this->mutex); + + if (!this->selectedEntity.has_value()) + { + gzdbg << "No link selected" << std::endl; + return; + } + + // Force and torque in world coordinates + math::Vector3d forceToApply = this->linkWorldPose.Rot().RotateVector( + _applyForce ? this->force : math::Vector3d::Zero); + math::Vector3d torqueToApply = this->linkWorldPose.Rot().RotateVector( + _applyTorque ? this->torque : math::Vector3d::Zero) + + this->inertialPos.Cross(forceToApply); + + msgs::EntityWrench msg; + msg.mutable_entity()->set_id(*this->selectedEntity); + msgs::Set(msg.mutable_wrench()->mutable_force(), forceToApply); + msgs::Set(msg.mutable_wrench()->mutable_torque(), torqueToApply); + + this->pub.Publish(msg); +} + +///////////////////////////////////////////////// +void ApplyForceTorquePrivate::OnRender() +{ + if (!this->scene) + { + // Get scene and user camera + this->scene = rendering::sceneFromFirstRenderEngine(); + if (!this->scene) + { + return; + } + + for (unsigned int i = 0; i < this->scene->SensorCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + this->scene->SensorByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + gzdbg << "ApplyForceTorque plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + + this->ray = this->scene->CreateRayQuery(); + + // Make material into overlay + auto mat = this->scene->Material("Default/TransRed")->Clone(); + mat->SetDepthCheckEnabled(false); + mat->SetDepthWriteEnabled(false); + + // Create visuals + if (!this->wrenchVis.Init(this->scene)) + { + gzerr << "Invalid scene" << std::endl; + return; + } + this->forceVisual = this->wrenchVis.CreateForceVisual(mat); + this->torqueVisual = this->wrenchVis.CreateTorqueVisual(mat); + this->gizmoVisual = this->scene->CreateGizmoVisual(); + this->scene->RootVisual()->AddChild(this->gizmoVisual); + } + + this->HandleMouseEvents(); + + this->UpdateVisuals(); + + if (this->sendBlockOrbit) + { + // Events with false should only be sent once + if (!this->blockOrbit) + this->sendBlockOrbit = false; + + gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &blockOrbitEvent); + } +} + +///////////////////////////////////////////////// +void ApplyForceTorquePrivate::UpdateVisuals() +{ + // Update force visualization + if (this->force != math::Vector3d::Zero && + this->selectedEntity.has_value()) + { + math::Vector3d worldForce = + this->linkWorldPose.Rot().RotateVector(this->force); + math::Vector3d applicationPoint = this->linkWorldPose.Pos() + + this->inertialPos + this->linkWorldPose.Rot().RotateVector(this->offset); + double scale = applicationPoint.Distance(this->camera->WorldPose().Pos()) + / 2.0; + this->wrenchVis.UpdateVectorVisual( + this->forceVisual, worldForce, applicationPoint, scale, true); + this->forceVisual->SetVisible(true); + } + else + { + this->forceVisual->SetVisible(false); + } + + // Update torque visualization + if (this->torque != math::Vector3d::Zero && + this->selectedEntity.has_value()) + { + math::Vector3d worldTorque = + this->linkWorldPose.Rot().RotateVector(this->torque); + math::Vector3d applicationPoint = + this->linkWorldPose.Pos() + this->inertialPos; + double scale = applicationPoint.Distance(this->camera->WorldPose().Pos()) + / 2.0; + this->wrenchVis.UpdateVectorVisual( + this->torqueVisual, worldTorque, applicationPoint, scale, false); + this->torqueVisual->SetVisible(true); + } + else + { + this->torqueVisual->SetVisible(false); + } + + // Update gizmo visual + if (this->activeVector != RotationToolVector::NONE) + { + math::Vector3d pos; + if (this->activeVector == RotationToolVector::FORCE + && this->force != math::Vector3d::Zero) + { + pos = + this->linkWorldPose.Pos() + this->inertialPos + + this->linkWorldPose.Rot().RotateVector(this->offset); + } + else if (this->activeVector == RotationToolVector::TORQUE + && this->torque != math::Vector3d::Zero) + { + pos = this->linkWorldPose.Pos() + this->inertialPos; + } + else + { + this->activeVector = RotationToolVector::NONE; + this->gizmoVisual->SetTransformMode(rendering::TransformMode::TM_NONE); + this->gizmoVisual->SetActiveAxis(math::Vector3d::Zero); + return; + } + + double scale = pos.Distance(this->camera->WorldPose().Pos()) + / 2.0; + + // TODO(anyone): use GizmoVisual::LookAt function from + // https://github.com/gazebosim/gz-rendering/pull/882 + + // Update gizmo visual rotation so that they are always facing the + // eye position and alligned with the active vector + math::Quaterniond gizmoRot = this->linkWorldPose.Rot() * this->vectorRot; + math::Vector3d eye = gizmoRot.RotateVectorReverse( + (this->camera->WorldPosition() - pos).Normalized()); + + rendering::VisualPtr xVis = this->gizmoVisual->ChildByAxis( + rendering::TransformAxis::TA_ROTATION_X); + xVis->SetVisible(false); + + rendering::VisualPtr yVis = this->gizmoVisual->ChildByAxis( + rendering::TransformAxis::TA_ROTATION_Y); + math::Vector3d yRot(0, atan2(eye.X(), eye.Z()), 0); + math::Vector3d yRotOffset(GZ_PI * 0.5, -GZ_PI * 0.5, 0); + yVis->SetWorldRotation(gizmoRot * + math::Quaterniond(yRot) * math::Quaterniond(yRotOffset)); + + rendering::VisualPtr zVis = this->gizmoVisual->ChildByAxis( + rendering::TransformAxis::TA_ROTATION_Z); + math::Vector3d zRot(0, 0, atan2(eye.Y(), eye.X())); + zVis->SetWorldRotation(gizmoRot * math::Quaterniond(zRot)); + + rendering::VisualPtr circleVis = this->gizmoVisual->ChildByAxis( + rendering::TransformAxis::TA_ROTATION_Z << 1); + math::Matrix4d lookAt; + lookAt = lookAt.LookAt(this->camera->WorldPosition(), pos); + math::Vector3d circleRotOffset(0, GZ_PI * 0.5, 0); + circleVis->SetWorldRotation( + lookAt.Rotation() * math::Quaterniond(circleRotOffset)); + + this->gizmoVisual->SetLocalScale(1.5 * scale); + this->gizmoVisual->SetTransformMode(rendering::TransformMode::TM_ROTATION); + this->gizmoVisual->SetLocalPosition(pos); + + if (this->activeAxis == rendering::TransformAxis::TA_ROTATION_Y) + this->gizmoVisual->SetActiveAxis(math::Vector3d::UnitY); + else if (this->activeAxis == rendering::TransformAxis::TA_ROTATION_Z) + this->gizmoVisual->SetActiveAxis(math::Vector3d::UnitZ); + else + this->gizmoVisual->SetActiveAxis(math::Vector3d::Zero); + } + else + { + this->gizmoVisual->SetTransformMode(rendering::TransformMode::TM_NONE); + this->gizmoVisual->SetActiveAxis(math::Vector3d::Zero); + } +} + +///////////////////////////////////////////////// +void ApplyForceTorquePrivate::HandleMouseEvents() +{ + // check for mouse events + if (!this->mouseDirty) + return; + this->mouseDirty = false; + + // handle mouse movements + if (this->mouseEvent.Button() == common::MouseEvent::LEFT) + { + // Rotating vector around the clicked axis + if (this->mouseEvent.Type() == common::MouseEvent::PRESS + && this->activeVector != RotationToolVector::NONE) + { + this->mousePressPos = this->mouseEvent.Pos(); + + // get the visual at mouse position + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (visual) + { + // check if the visual is an axis in the gizmo visual + auto axis = this->gizmoVisual->AxisById(visual->Id()); + if (axis == rendering::TransformAxis::TA_ROTATION_Y + || axis == rendering::TransformAxis::TA_ROTATION_Z) + { + if (this->activeVector == RotationToolVector::FORCE) + this->initialVector = this->force; + else if (this->activeVector == RotationToolVector::TORQUE) + this->initialVector = this->torque; + else + return; + this->initialVectorRot = this->vectorRot; + this->blockOrbit = true; + this->sendBlockOrbit = true; + this->activeAxis = axis; + } + else + { + this->blockOrbit = false; + this->sendBlockOrbit = true; + this->activeAxis = rendering::TransformAxis::TA_NONE; + return; + } + } + } + else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + this->blockOrbit = false; + this->sendBlockOrbit = true; + this->activeAxis = rendering::TransformAxis::TA_NONE; + + if (!this->mouseEvent.Dragging()) + { + // get the visual at mouse position + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + if (!visual) + return; + + // Select a vector for the rotation tool + auto id = visual->Id(); + if ((this->forceVisual->Id() == id || + this->forceVisual->ChildById(id)) + && this->activeVector != RotationToolVector::FORCE) + { + this->activeVector = RotationToolVector::FORCE; + this->vectorRot = math::Matrix4d::LookAt( + -this->force, math::Vector3d::Zero).Rotation(); + } + else if ((this->torqueVisual->Id() == id || + this->torqueVisual->ChildById(id)) + && this->activeVector != RotationToolVector::TORQUE) + { + this->activeVector = RotationToolVector::TORQUE; + this->vectorRot = math::Matrix4d::LookAt( + -this->torque, math::Vector3d::Zero).Rotation(); + } + else if (this->gizmoVisual->AxisById(visual->Id()) == + rendering::TransformAxis::TA_NONE) + { + this->activeVector = RotationToolVector::NONE; + } + } + } + } + if (this->mouseEvent.Type() == common::MouseEvent::MOVE + && this->activeAxis != rendering::TransformAxis::TA_NONE) + { + this->blockOrbit = true; + this->sendBlockOrbit = true; + + auto imageWidth = static_cast(this->camera->ImageWidth()); + auto imageHeight = static_cast(this->camera->ImageHeight()); + double nx = 2.0 * this->mousePressPos.X() / imageWidth - 1.0; + double ny = 1.0 - 2.0 * this->mousePressPos.Y() / imageHeight; + double nxEnd = 2.0 * this->mouseEvent.Pos().X() / imageWidth - 1.0; + double nyEnd = 1.0 - 2.0 * this->mouseEvent.Pos().Y() / imageHeight; + math::Vector2d start(nx, ny); + math::Vector2d end(nxEnd, nyEnd); + + // Axis of rotation in world frame + math::Vector3d axis; + if (this->activeAxis == rendering::TransformAxis::TA_ROTATION_Y) + { + axis = this->linkWorldPose.Rot().RotateVector(this->vectorRot.YAxis()); + } + else if (this->activeAxis == rendering::TransformAxis::TA_ROTATION_Z) + { + axis = this->linkWorldPose.Rot().RotateVector(this->vectorRot.ZAxis()); + } + + /// get start and end pos in world frame from 2d point + math::Vector3d pos = this->linkWorldPose.Pos() + this->inertialPos + + this->linkWorldPose.Rot().RotateVector(this->offset); + double d = pos.Dot(axis); + math::Planed plane(axis, d); + + math::Vector3d startPos; + this->ray->SetFromCamera(this->camera, start); + if (auto v{plane.Intersection( + this->ray->Origin(), this->ray->Direction())}) + startPos = *v; + else + return; + + math::Vector3d endPos; + this->ray->SetFromCamera(this->camera, end); + if (auto v{plane.Intersection( + this->ray->Origin(), this->ray->Direction())}) + endPos = *v; + else + return; + + // get vectors from link pos to both points + startPos = (startPos - pos).Normalized(); + endPos = (endPos - pos).Normalized(); + // compute angle between two vectors + double signTest = startPos.Cross(endPos).Dot(axis); + double angle = atan2( + (startPos.Cross(endPos)).Length(), startPos.Dot(endPos)); + if (signTest < 0) + angle = -angle; + + // Desired rotation in link frame + axis = this->linkWorldPose.Rot().RotateVectorReverse(axis); + math::Quaterniond rot(axis, angle); + this->vectorRot = rot * this->initialVectorRot; + math::Vector3d newVector = + this->initialVector.Length() * this->vectorRot.XAxis(); + + if (this->activeVector == RotationToolVector::FORCE) + this->force = newVector; + else if (this->activeVector == RotationToolVector::TORQUE) + this->torque = newVector; + this->vectorDirty = true; + } +} + +// Register this plugin +GZ_ADD_PLUGIN(ApplyForceTorque, gz::gui::Plugin) diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh new file mode 100644 index 0000000000..7588915082 --- /dev/null +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh @@ -0,0 +1,159 @@ +/* + * Copyright (C) 2023 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 GZ_GUI_APPLYFORCETORQUE_HH_ +#define GZ_GUI_APPLYFORCETORQUE_HH_ + +#include + +#include +#include +#include +#include + +#include +#include + +namespace gz +{ +namespace sim +{ + class ApplyForceTorquePrivate; + + /// \brief Publish wrench to "/world//wrench" topic. + /// Automatically loads the ApplyLinkWrench system. + /// + /// ## Configuration + /// This plugin doesn't accept any custom configuration. + class ApplyForceTorque : public gz::sim::GuiSystem + { + Q_OBJECT + + /// \brief Model name + Q_PROPERTY( + QString modelName + READ ModelName + NOTIFY ModelNameChanged + ) + + /// \brief Link list + Q_PROPERTY( + QStringList linkNameList + READ LinkNameList + NOTIFY LinkNameListChanged + ) + + /// \brief Link index + Q_PROPERTY( + int linkIndex + READ LinkIndex + WRITE SetLinkIndex + NOTIFY LinkIndexChanged + ) + + /// \brief Force + Q_PROPERTY( + QVector3D force + READ Force + WRITE SetForce + NOTIFY ForceChanged + ) + + /// \brief Torque + Q_PROPERTY( + QVector3D torque + READ Torque + WRITE SetTorque + NOTIFY TorqueChanged + ) + + /// \brief Constructor + public: ApplyForceTorque(); + + /// \brief Destructor + public: ~ApplyForceTorque() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Get the name of the selected model + public: Q_INVOKABLE QString ModelName() const; + + /// \brief Notify that the model name changed + signals: void ModelNameChanged(); + + /// \brief Get the name of the links of the selected model + public: Q_INVOKABLE QStringList LinkNameList() const; + + /// \brief Notify that the link list changed + signals: void LinkNameListChanged(); + + /// \brief Get index of the link in the list + public: Q_INVOKABLE int LinkIndex() const; + + /// \brief Notify that the link index changed + signals: void LinkIndexChanged(); + + /// \brief Set the index of the link in the list + public: Q_INVOKABLE void SetLinkIndex(int _linkIndex); + + /// \brief Get the force vector + /// \return The force vector + public: Q_INVOKABLE QVector3D Force() const; + + /// \brief Notify that the force changed + signals: void ForceChanged(); + + /// \brief Set the force vector + /// \param[in] _force The new force vector + public: Q_INVOKABLE void SetForce(QVector3D _force); + + /// \brief Get the torque vector + /// \return The torque vector + public: Q_INVOKABLE QVector3D Torque() const; + + /// \brief Notify that the torque changed + signals: void TorqueChanged(); + + /// \brief Set the torque vector + /// \param[in] _torque The new torque vector + public: Q_INVOKABLE void SetTorque(QVector3D _torque); + + /// \brief Apply the specified force + public: Q_INVOKABLE void ApplyForce(); + + /// \brief Apply the specified torque + public: Q_INVOKABLE void ApplyTorque(); + + /// \brief Apply the specified force and torque + public: Q_INVOKABLE void ApplyAll(); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml new file mode 100644 index 0000000000..d3cc802760 --- /dev/null +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml @@ -0,0 +1,253 @@ +/* + * Copyright (C) 2023 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 8 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 700 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + // Maximum value for GzSpinBoxes + property int maxValue: 1000000 + + // Minimum value for GzSpinBoxes + property int minValue: -1000000 + + // Precision of the GzSpinBoxes in decimal places + property int decimalPlaces: 2 + + // Step size of the GzSpinBoxes + property double step: 1.0 + + Label { + Layout.columnSpan: 8 + Layout.fillWidth: true + wrapMode: Text.WordWrap + id: frameText + text: "Forces and torques are given in link-fixed frame." + } + + Label { + Layout.columnSpan: 8 + Layout.fillWidth: true + wrapMode: Text.WordWrap + id: rotText + text: "Click on an arrow to toggle its rotation tool." + } + + Text { + Layout.columnSpan: 2 + id: modelText + color: "dimgrey" + text: qsTr("Model:") + } + + Text { + Layout.columnSpan: 6 + id: modelName + color: "dimgrey" + text: ApplyForceTorque.modelName + } + + Text { + Layout.columnSpan: 2 + id: linkText + color: "dimgrey" + text: qsTr("Link:") + } + + ComboBox { + Layout.columnSpan: 6 + id: linkCombo + Layout.fillWidth: true + model: ApplyForceTorque.linkNameList + currentIndex: ApplyForceTorque.linkIndex + onCurrentIndexChanged: { + ApplyForceTorque.linkIndex = currentIndex + } + } + + // Force + Text { + Layout.columnSpan: 8 + id: forceText + color: "dimgrey" + text: qsTr("Force (applied to the center of mass):") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: forceXText + color: "dimgrey" + text: qsTr("X (N)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: forceX + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.force.x + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.force.x = forceX.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: forceYText + color: "dimgrey" + text: qsTr("Y (N)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: forceY + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.force.y + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.force.y = forceY.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: forceZText + color: "dimgrey" + text: qsTr("Z (N)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: forceZ + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.force.z + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.force.z = forceZ.value + } + + Button { + text: qsTr("Apply Force") + Layout.columnSpan: 8 + Layout.fillWidth: true + onClicked: function() { + ApplyForceTorque.ApplyForce() + } + } + + // Torque + Text { + Layout.columnSpan: 8 + id: torqueText + color: "dimgrey" + text: qsTr("Torque:") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: torqueXText + color: "dimgrey" + text: qsTr("X (N.m)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: torqueX + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.torque.x + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.torque.x = torqueX.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: torqueYText + color: "dimgrey" + text: qsTr("Y (N.m)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: torqueY + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.torque.y + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.torque.y = torqueY.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: torqueZText + color: "dimgrey" + text: qsTr("Z (N.m)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: torqueZ + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.torque.z + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.torque.z = torqueZ.value + } + + Button { + text: qsTr("Apply Torque") + Layout.columnSpan: 8 + Layout.fillWidth: true + onClicked: function() { + ApplyForceTorque.ApplyTorque() + } + } + + Button { + text: qsTr("Apply All") + Layout.columnSpan: 8 + Layout.fillWidth: true + onClicked: function() { + ApplyForceTorque.ApplyAll() + } + } +} diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qrc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qrc new file mode 100644 index 0000000000..b95739311d --- /dev/null +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qrc @@ -0,0 +1,5 @@ + + + ApplyForceTorque.qml + + diff --git a/src/gui/plugins/apply_force_torque/CMakeLists.txt b/src/gui/plugins/apply_force_torque/CMakeLists.txt new file mode 100644 index 0000000000..db13a65144 --- /dev/null +++ b/src/gui/plugins/apply_force_torque/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_gui_plugin(ApplyForceTorque + SOURCES ApplyForceTorque.cc + QT_HEADERS ApplyForceTorque.hh + PUBLIC_LINK_LIBS + ${PROJECT_LIBRARY_TARGET_NAME}-rendering +) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 2f8fed736d..52f77879e0 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -765,6 +765,13 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::PhysicsEnginePlugin::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::PhysicsSolver::typeId) { auto comp = _ecm.Component( @@ -1223,7 +1230,7 @@ void ComponentInspector::QuerySystems() "/system/info"}; if (!this->dataPtr->node.Request(service, req, timeout, res, result)) { - ignerr << "Unable to query available systems." << std::endl; + gzerr << "Unable to query available systems." << std::endl; return; } @@ -1233,8 +1240,8 @@ void ComponentInspector::QuerySystems() { if (plugin.filename().empty()) { - ignerr << "Received empty plugin name. This shouldn't happen." - << std::endl; + gzerr << "Received empty plugin name. This shouldn't happen." + << std::endl; continue; } @@ -1281,7 +1288,7 @@ void ComponentInspector::OnAddSystem(const QString &_name, auto it = this->dataPtr->systemMap.find(filenameStr); if (it == this->dataPtr->systemMap.end()) { - ignerr << "Internal error: failed to find [" << filenameStr + gzerr << "Internal error: failed to find [" << filenameStr << "] in system map." << std::endl; return; } @@ -1304,11 +1311,11 @@ void ComponentInspector::OnAddSystem(const QString &_name, "/entity/system/add"}; if (!this->dataPtr->node.Request(service, req, timeout, res, result)) { - ignerr << "Error adding new system to entity: " - << this->dataPtr->entity << "\n" - << "Name: " << name << "\n" - << "Filename: " << filename << "\n" - << "Inner XML: " << innerxml << std::endl; + gzerr << "Error adding new system to entity: " + << this->dataPtr->entity << "\n" + << "Name: " << name << "\n" + << "Filename: " << filename << "\n" + << "Inner XML: " << innerxml << std::endl; } } diff --git a/src/gui/plugins/mouse_drag/CMakeLists.txt b/src/gui/plugins/mouse_drag/CMakeLists.txt new file mode 100644 index 0000000000..9d34ef4653 --- /dev/null +++ b/src/gui/plugins/mouse_drag/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_gui_plugin(MouseDrag + SOURCES MouseDrag.cc + QT_HEADERS MouseDrag.hh +) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc new file mode 100644 index 0000000000..44b0cc3e35 --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -0,0 +1,839 @@ +/* + * Copyright (C) 2023 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. + * +*/ + +#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 "MouseDrag.hh" + +namespace gz +{ +namespace sim +{ + /// \enum MouseDragMode + /// \brief Unique identifiers for mouse dragging modes + enum MouseDragMode + { + /// \brief Inactive state + NONE = 0, + /// \brief Rotation mode + ROTATE = 1, + /// \brief Translation mode + TRANSLATE = 2, + }; + + class MouseDragPrivate + { + /// \brief Handle mouse events + public: void HandleMouseEvents(); + + /// \brief Perform rendering calls in the rendering thread. + public: void OnRender(); + + /// \brief Update the gains for the controller + /// \param[in] _inertial Inertial of the link + public: void UpdateGains(const math::Inertiald &_inertial); + + /// \brief Calculate the wrench to be applied to the link, + /// based on a spring-damper control law. + /// + /// This attempts to minimize the error (in position or + /// rotation, depending on the mode) with a proportional control, while + /// damping the rate of change of the link's pose. + /// + /// \param[in] _error Position error in translation mode, Rotation error + /// (in Euler angles) in rotation mode + /// \param[in] _dt Change in time since last call + /// \param[in] _linkWorldPose World pose of the link + /// \param[in] _inertial inertial of the link + /// \return The EntityWrench message containing the calculated + /// force and torque + public: msgs::EntityWrench CalculateWrench( + const math::Vector3d &_error, + const std::chrono::duration &_dt, + const math::Pose3d &_linkWorldPose); + + /// \brief Sets the RayQuery from the user camera to the given position + /// on the screen + /// \param[in] _pos Position on the screen + public: void SetRayFromCamera(const math::Vector2i &_pos); + + /// \brief Corrects an angle so that it is in the [-pi; pi] interval, + /// in order to eliminate the discontinuity around 0 and 2pi + /// \param[in] _angle The angle in radians + /// \return The corrected angle + public: double CorrectAngle(const double _angle); + + /// \brief Transport node + public: transport::Node node; + + /// \brief Publisher for EntityWrench messages + public: transport::Node::Publisher pub; + + /// \brief To synchronize member access + public: std::mutex mutex; + + /// \brief World name + public: std::string worldName; + + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + + /// \brief Ray query for mouse clicks + public: rendering::RayQueryPtr rayQuery{nullptr}; + + /// \brief Holds the latest mouse event + public: gz::common::MouseEvent mouseEvent; + + /// \brief Initial position of a mouse click + public: math::Vector2i mousePressPos; + + /// \brief True if there are new mouse events to process + public: bool mouseDirty{false}; + + /// \brief True if there are new dragging mode changes to process + public: bool modeDirty{false}; + + /// \brief True if the force should be applied to the center of mass + public: bool applyCOM{false}; + + /// \brief True if camera orbit should be blocked + public: bool blockOrbit{false}; + + /// \brief True if BlockOrbit events should be sent + public: bool sendBlockOrbit{false}; + + /// \brief True if dragging is active + public: bool dragActive{false}; + + /// \brief True if the ApplyLinkWrench system is loaded + public: bool systemLoaded{false}; + + /// \brief Current dragging mode + public: MouseDragMode mode = MouseDragMode::NONE; + + /// \brief Link to which the wrenches are applied + public: Entity linkId{kNullEntity}; + + /// \brief Plane of force application + public: math::Planed plane; + + /// \brief Application point of the wrench in world coordinates + public: math::Vector3d applicationPoint; + + /// \brief Initial world rotation of the link during mouse click + public: math::Quaterniond initialRot; + + /// \brief Point to which the link is dragged in translation mode + public: math::Vector3d target; + + /// \brief Goal link rotation for rotation mode + public: math::Quaterniond goalRot; + + /// \brief Offset of the force application point relative to the + /// link origin, expressed in the link-fixed frame + public: math::Vector3d offset{0.0, 0.0, 0.0}; + + /// \brief Link world pose in previous Update call. Used for damping + public: math::Pose3d poseLast; + + /// \brief Spring stiffness for translation, in (m/s²)/m + public: double posStiffness{100.0}; + + /// \brief P-gain for translation + public: double pGainPos{0.0}; + + /// \brief D-gain for translation + public: double dGainPos{0.0}; + + /// \brief Spring stiffness for rotation, in (rad/s²)/rad + public: double rotStiffness{200.0}; + + /// \brief P-gain for rotation + public: double pGainRot{0.0}; + + /// \brief D-gain for rotation + public: double dGainRot{0.0}; + + /// \brief Arrow for visualizing force in translation mode. + /// This arrow goes from the application point to the target point. + public: rendering::ArrowVisualPtr arrowVisual{nullptr}; + + /// \brief Box for visualizing rotation mode + public: rendering::VisualPtr boxVisual{nullptr}; + + /// \brief Size of the bounding box of the selected link + public: math::Vector3d bboxSize; + }; +} +} + +using namespace gz; +using namespace sim; + +///////////////////////////////////////////////// +MouseDrag::MouseDrag() + : GuiSystem(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +MouseDrag::~MouseDrag() = default; + +///////////////////////////////////////////////// +void MouseDrag::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "Mouse drag"; + + // Create wrench publisher + const auto worldNames = gz::gui::worldNames(); + if (!worldNames.empty()) + { + this->dataPtr->worldName = worldNames[0].toStdString(); + const auto topic = transport::TopicUtils::AsValidTopic( + "/world/" + this->dataPtr->worldName + "/wrench"); + if (topic.empty()) + { + gzerr << "Unable to create publisher" << std::endl; + return; + } + this->dataPtr->pub = + this->dataPtr->node.Advertise(topic); + gzdbg << "Created publisher to " << topic << std::endl; + } + else + { + gzerr << "Unable to find world" << std::endl; + return; + } + + // Read configuration + if (_pluginElem) + { + if (auto elem = _pluginElem->FirstChildElement("rotation_stiffness")) + { + elem->QueryDoubleText(&this->dataPtr->rotStiffness); + emit this->RotStiffnessChanged(); + } + + if (auto elem = _pluginElem->FirstChildElement("position_stiffness")) + { + elem->QueryDoubleText(&this->dataPtr->posStiffness); + emit this->PosStiffnessChanged(); + } + } + + gz::gui::App()->findChild + ()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gz::gui::events::Render::kType) + { + this->dataPtr->OnRender(); + } + else if (_event->type() == gz::gui::events::LeftClickOnScene::kType) + { + // Mutex can't be locked on the whole eventFilter because that causes + // the program to freeze, since the Update function sends GUI events + std::lock_guard lock(this->dataPtr->mutex); + const auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::RightClickOnScene::kType) + { + std::lock_guard lock(this->dataPtr->mutex); + const auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::MousePressOnScene::kType) + { + std::lock_guard lock(this->dataPtr->mutex); + const auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::DragOnScene::kType) + { + std::lock_guard lock(this->dataPtr->mutex); + const auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + + this->dataPtr->HandleMouseEvents(); + + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void MouseDrag::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("MouseDrag::Update"); + // Load the ApplyLinkWrench system + if (!this->dataPtr->systemLoaded) + { + const std::string name{"gz::sim::systems::ApplyLinkWrench"}; + const std::string filename{"gz-sim-apply-link-wrench-system"}; + const std::string innerxml{"0"}; + + // Get world entity + Entity worldEntity; + _ecm.Each( + [&](const Entity &_entity, + const components::World */*_world*/, + const components::Name *_name)->bool + { + if (_name->Data() == this->dataPtr->worldName) + { + worldEntity = _entity; + return false; + } + return true; + }); + + // Check if already loaded + const auto msg = + _ecm.ComponentData(worldEntity); + if (!msg) + { + gzdbg << "Unable to find SystemPluginInfo component for entity " + << worldEntity << std::endl; + return; + } + for (const auto &plugin : msg->plugins()) + { + if (plugin.filename() == filename) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system already loaded" << std::endl; + break; + } + } + + // Request to load system + if (!this->dataPtr->systemLoaded) + { + msgs::EntityPlugin_V req; + req.mutable_entity()->set_id(worldEntity); + auto plugin = req.add_plugins(); + plugin->set_name(name); + plugin->set_filename(filename); + plugin->set_innerxml(innerxml); + + msgs::Boolean res; + bool result; + const unsigned int timeout = 5000; + const auto service = transport::TopicUtils::AsValidTopic( + "/world/" + this->dataPtr->worldName + "/entity/system/add"); + if (service.empty()) + { + gzerr << "Unable to request " << service << std::endl; + return; + } + if (this->dataPtr->node.Request(service, req, timeout, res, result)) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system has been loaded" << std::endl; + } + else + { + gzerr << "Error adding new system to entity: " + << worldEntity << "\n" + << "Name: " << name << "\n" + << "Filename: " << filename << "\n" + << "Inner XML: " << innerxml << std::endl; + return; + } + } + } + + std::lock_guard lock(this->dataPtr->mutex); + + if (this->dataPtr->mode == MouseDragMode::NONE || + _info.paused) + { + this->dataPtr->mode = MouseDragMode::NONE; + this->dataPtr->dragActive = false; + return; + } + + // Get Link corresponding to clicked Visual + const Link link(this->dataPtr->linkId); + const auto model = link.ParentModel(_ecm); + const auto linkWorldPose = worldPose(this->dataPtr->linkId, _ecm); + const auto inertial = + _ecm.Component(this->dataPtr->linkId); + if (!link.Valid(_ecm) || !inertial || + !model->Valid(_ecm) || model->Static(_ecm)) + { + this->dataPtr->blockOrbit = false; + return; + } + + if (this->dataPtr->blockOrbit) + this->dataPtr->sendBlockOrbit = true; + + this->dataPtr->dragActive = true; + + if (this->dataPtr->modeDirty) + { + this->dataPtr->modeDirty = false; + + gui::events::DeselectAllEntities event(true); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &event); + + this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); + this->dataPtr->initialRot = linkWorldPose.Rot(); + this->dataPtr->poseLast = linkWorldPose; + this->dataPtr->UpdateGains(inertial->Data()); + + // Calculate offset of force application from link origin + if (this->dataPtr->applyCOM || this->dataPtr->mode == MouseDragMode::ROTATE) + { + this->dataPtr->offset = inertial->Data().Pose().Pos(); + this->dataPtr->applicationPoint = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + } + else + { + this->dataPtr->offset = linkWorldPose.Rot().RotateVectorReverse( + this->dataPtr->applicationPoint - linkWorldPose.Pos()); + } + + // The plane of wrench application should be normal to the center + // of the camera view and pass through the application point + const math::Vector3d direction = + this->dataPtr->camera->WorldPose().Rot().XAxis(); + const double planeOffset = + direction.Dot(this->dataPtr->applicationPoint); + this->dataPtr->plane = math::Planed(direction, planeOffset); + } + // Track the application point + else + { + this->dataPtr->applicationPoint = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + } + + // Make a ray query at the mouse position + this->dataPtr->SetRayFromCamera(this->dataPtr->mouseEvent.Pos()); + const math::Vector3d end = this->dataPtr->rayQuery->Direction(); + + // Wrench in world coordinates, applied at the link origin + msgs::EntityWrench msg; + if (this->dataPtr->mode == MouseDragMode::ROTATE) + { + // Calculate rotation angle from mouse displacement + this->dataPtr->SetRayFromCamera(this->dataPtr->mousePressPos); + const math::Vector3d start = this->dataPtr->rayQuery->Direction(); + math::Vector3d axis = start.Cross(end); + const double angle = -atan2(axis.Length(), start.Dot(end)); + + // Project rotation axis onto plane + axis -= axis.Dot(this->dataPtr->plane.Normal()) * + this->dataPtr->plane.Normal(); + axis.Normalize(); + + // Calculate the necessary rotation and torque + this->dataPtr->goalRot = + math::Quaterniond(axis, angle) * this->dataPtr->initialRot; + const math::Quaterniond errorRot = + linkWorldPose.Rot() * this->dataPtr->goalRot.Inverse(); + msg = this->dataPtr->CalculateWrench( + errorRot.Euler(), _info.dt, linkWorldPose); + } + else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + { + const math::Vector3d origin = this->dataPtr->rayQuery->Origin(); + if (const auto t = this->dataPtr->plane.Intersection(origin, end)) + this->dataPtr->target = *t; + else + return; + + const math::Vector3d errorPos = + this->dataPtr->applicationPoint - this->dataPtr->target; + msg = this->dataPtr->CalculateWrench(errorPos, _info.dt, linkWorldPose); + } + + // Publish wrench + this->dataPtr->pub.Publish(msg); +} + +///////////////////////////////////////////////// +void MouseDrag::OnSwitchCOM(const bool _checked) +{ + this->dataPtr->applyCOM = _checked; +} + +///////////////////////////////////////////////// +double MouseDrag::RotStiffness() const +{ + return this->dataPtr->rotStiffness; +} + +///////////////////////////////////////////////// +void MouseDrag::SetRotStiffness(double _rotStiffness) +{ + this->dataPtr->rotStiffness = _rotStiffness; +} + +///////////////////////////////////////////////// +double MouseDrag::PosStiffness() const +{ + return this->dataPtr->posStiffness; +} + +///////////////////////////////////////////////// +void MouseDrag::SetPosStiffness(double _posStiffness) +{ + this->dataPtr->posStiffness = _posStiffness; +} + +///////////////////////////////////////////////// +void MouseDragPrivate::OnRender() +{ + std::lock_guard lock(this->mutex); + + // Get scene and user camera + if (!this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (!this->scene) + { + return; + } + + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) + { + const auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + gzdbg << "MouseDrag plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + + if (!this->camera) + { + gzerr << "MouseDrag camera is not available" << std::endl; + return; + } + this->rayQuery = this->scene->CreateRayQuery(); + + auto mat = this->scene->Material("Default/TransRed")->Clone(); + mat->SetDepthCheckEnabled(false); + mat->SetDepthWriteEnabled(false); + + this->arrowVisual = this->scene->CreateArrowVisual(); + this->arrowVisual->SetMaterial(mat); + this->arrowVisual->ShowArrowHead(true); + this->arrowVisual->ShowArrowShaft(true); + this->arrowVisual->ShowArrowRotation(false); + + this->boxVisual = scene->CreateVisual(); + this->boxVisual->AddGeometry(scene->CreateBox()); + this->boxVisual->SetInheritScale(false); + this->boxVisual->SetMaterial(mat); + this->boxVisual->SetUserData("gui-only", true); + } + + // Update the visualization + if (!this->dragActive) + { + this->arrowVisual->SetVisible(false); + this->boxVisual->SetVisible(false); + } + else if (this->mode == MouseDragMode::ROTATE) + { + this->boxVisual->SetLocalPosition(this->applicationPoint); + this->boxVisual->SetLocalRotation(this->goalRot); + this->boxVisual->SetLocalScale(1.2 * this->bboxSize); + + this->arrowVisual->SetVisible(false); + this->boxVisual->SetVisible(true); + } + else if (this->mode == MouseDragMode::TRANSLATE) + { + const math::Vector3d axisDir = this->target - this->applicationPoint; + math::Quaterniond quat; + quat.SetFrom2Axes(math::Vector3d::UnitZ, axisDir); + const double scale = 2 * this->bboxSize.Length(); + this->arrowVisual->SetLocalPosition(this->applicationPoint); + this->arrowVisual->SetLocalRotation(quat); + this->arrowVisual->SetLocalScale(scale, scale, axisDir.Length() / 0.75); + + this->arrowVisual->SetVisible(true); + this->boxVisual->SetVisible(false); + } + + if (this->sendBlockOrbit) + { + // Events with false should only be sent once + if (!this->blockOrbit) + this->sendBlockOrbit = false; + + gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &blockOrbitEvent); + } +} + +///////////////////////////////////////////////// +void MouseDragPrivate::HandleMouseEvents() +{ + // Check for mouse events + if (!this->mouseDirty) + { + return; + } + this->mouseDirty = false; + + std::lock_guard lock(this->mutex); + + if (this->mouseEvent.Type() == common::MouseEvent::PRESS && + this->mouseEvent.Control() && + this->mouseEvent.Button() != common::MouseEvent::MIDDLE) + { + // Get the visual at mouse position + const rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (!visual || !visual->Parent()) + { + this->mode = MouseDragMode::NONE; + return; + } + try + { + this->linkId = + std::get(visual->Parent()->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e) + { + this->mode = MouseDragMode::NONE; + return; + } + + this->blockOrbit = true; + this->modeDirty = true; + + // Get the 3D coordinates of the clicked point + this->applicationPoint = rendering::screenToScene( + this->mouseEvent.Pos(), this->camera, + this->rayQuery); + + this->bboxSize = visual->LocalBoundingBox().Size(); + + if (this->mouseEvent.Button() == common::MouseEvent::LEFT) + { + this->mode = MouseDragMode::ROTATE; + } + else if (this->mouseEvent.Button() == common::MouseEvent::RIGHT) + { + this->mode = MouseDragMode::TRANSLATE; + } + } + else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + this->mode = MouseDragMode::NONE; + this->dragActive = false; + this->blockOrbit = false; + } + else if (this->mouseEvent.Type() == common::MouseEvent::MOVE) + { + if (this->mode != MouseDragMode::NONE) + { + this->blockOrbit = true; + } + } +} + +///////////////////////////////////////////////// +void MouseDragPrivate::UpdateGains(const math::Inertiald &_inertial) +{ + if (this->mode == MouseDragMode::ROTATE) + { + // TODO(anyone): is this the best way to scale rotation gains? + const double avgInertia = + _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + this->pGainRot = this->rotStiffness * avgInertia; + this->dGainRot = 2 * sqrt(this->rotStiffness) * avgInertia; + this->pGainPos = 0.0; + this->dGainPos = 0.0; + } + else if (this->mode == MouseDragMode::TRANSLATE) + { + const double mass = + _inertial.MassMatrix().Mass(); + this->pGainPos = this->posStiffness * mass; + this->dGainPos = 0.5 * sqrt(this->posStiffness) * mass; + this->pGainRot = 0.0; + + if (!this->applyCOM) + { + const double avgInertia = + _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + this->dGainRot = 0.5 * sqrt(this->rotStiffness) * avgInertia; + } + } +} + +///////////////////////////////////////////////// +msgs::EntityWrench MouseDragPrivate::CalculateWrench( + const math::Vector3d &_error, + const std::chrono::duration &_dt, + const math::Pose3d &_linkWorldPose) +{ + math::Vector3d force, torque; + if (this->mode == MouseDragMode::ROTATE) + { + math::Vector3d dErrorRot = + (_linkWorldPose.Rot() * this->poseLast.Rot().Inverse()).Euler() + / _dt.count(); + for (auto i : {0, 1, 2}) + { + dErrorRot[i] = this->CorrectAngle(dErrorRot[i]); + } + torque = - this->pGainRot * _error - this->dGainRot * dErrorRot; + } + else if (this->mode == MouseDragMode::TRANSLATE) + { + const math::Vector3d dErrorPos = + (_linkWorldPose.Pos() - this->poseLast.Pos()) / _dt.count(); + + force = - this->pGainPos * _error - this->dGainPos * dErrorPos; + torque = + _linkWorldPose.Rot().RotateVector(this->offset).Cross(force); + + // If the force is not applied to the center of mass, slightly damp the + // resulting rotation + if (!this->applyCOM) + { + math::Vector3d dErrorRot = + (_linkWorldPose.Rot() * this->poseLast.Rot().Inverse()).Euler() + / _dt.count(); + for (auto i : {0, 1, 2}) + { + dErrorRot[i] = this->CorrectAngle(dErrorRot[i]); + } + torque -= this->dGainRot * dErrorRot; + } + } + else + { + return msgs::EntityWrench(); + } + + this->poseLast = _linkWorldPose; + + msgs::EntityWrench msg; + msg.mutable_entity()->set_id(this->linkId); + msgs::Set(msg.mutable_wrench()->mutable_force(), force); + msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); + return msg; +} + +///////////////////////////////////////////////// +void MouseDragPrivate::SetRayFromCamera(const math::Vector2i &_pos) +{ + // Normalize position on the image + const double width = this->camera->ImageWidth(); + const double height = this->camera->ImageHeight(); + + const double nx = 2.0 * _pos.X() / width - 1.0; + const double ny = 1.0 - 2.0 * _pos.Y() / height; + + // Make a ray query at the given position + this->rayQuery->SetFromCamera(this->camera, math::Vector2d(nx, ny)); +} + + +double MouseDragPrivate::CorrectAngle(const double _angle) +{ + double result = _angle; + if (_angle > GZ_PI) + result -= 2 * GZ_PI; + else if (_angle < -GZ_PI) + result += 2 * GZ_PI; + return result; +} + +// Register this plugin +GZ_ADD_PLUGIN(MouseDrag, gz::gui::Plugin) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.hh b/src/gui/plugins/mouse_drag/MouseDrag.hh new file mode 100644 index 0000000000..6f61a31974 --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.hh @@ -0,0 +1,107 @@ +/* + * Copyright (C) 2023 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 GZ_GUI_MOUSEDRAG_HH +#define GZ_GUI_MOUSEDRAG_HH + +#include + +#include + +namespace gz +{ +namespace sim +{ + class MouseDragPrivate; + + /// \brief Translate and rotate links by dragging them with the mouse. + /// Automatically loads the ApplyLinkWrench system. + /// + /// ## Configuration + /// + /// * \ : Stiffness of rotation mode, defaults to 100 + /// * \ : Stiffness of translation mode, defaults to 100 + class MouseDrag : public gz::sim::GuiSystem + { + Q_OBJECT + + /// \brief Stiffness of rotation mode + Q_PROPERTY( + double rotStiffness + READ RotStiffness + WRITE SetRotStiffness + NOTIFY RotStiffnessChanged + ) + + /// \brief Stiffness of translation mode + Q_PROPERTY( + double posStiffness + READ PosStiffness + WRITE SetPosStiffness + NOTIFY PosStiffnessChanged + ) + + /// \brief Constructor + public: MouseDrag(); + + /// \brief Destructor + public: ~MouseDrag() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Callback when COM switch is pressed + /// \param[in] _checked True if force should be applied to center of mass + public slots: void OnSwitchCOM(const bool _checked); + + /// \brief Get the rotational stiffness + /// \return The rotational stiffness + public: Q_INVOKABLE double RotStiffness() const; + + /// \brief Notify that the rotational stiffness changed + signals: void RotStiffnessChanged(); + + /// \brief Set the rotational stiffness + /// \param[in] _rotStiffness The new rotational stiffness + public: Q_INVOKABLE void SetRotStiffness(double _rotStiffness); + + /// \brief Get the translational stiffness + /// \return The translational stiffness + public: Q_INVOKABLE double PosStiffness() const; + + /// \brief Notify that the translational stiffness changed + signals: void PosStiffnessChanged(); + + /// \brief Set the translational stiffness + /// \param[in] _posStiffness The new translational stiffness + public: Q_INVOKABLE void SetPosStiffness(double _posStiffness); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/mouse_drag/MouseDrag.qml b/src/gui/plugins/mouse_drag/MouseDrag.qml new file mode 100644 index 0000000000..505649ad01 --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.qml @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2023 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 8 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 300 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + // Maximum value for GzSpinBoxes + property int maxValue: 1000000 + + // Precision of the GzSpinBoxes in decimal places + property int decimalPlaces: 1 + + // Step size of the GzSpinBoxes + property double step: 10.0 + + Text { + Layout.columnSpan: 8 + id: rotationText + text: qsTr("Rotation (Ctrl+Left-Click)") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: rotStiffText + text: qsTr("Rotation stiffness") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: rotStiffness + maximumValue: maxValue + minimumValue: 0 + value: MouseDrag.rotStiffness + decimals: decimalPlaces + stepSize: step + onValueChanged: MouseDrag.rotStiffness = rotStiffness.value + } + + Text { + Layout.columnSpan: 8 + id: translationText + text: qsTr("Translation (Ctrl+Right-Click)") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: posStiffText + text: qsTr("Position stiffness") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: posStiffness + maximumValue: maxValue + minimumValue: 0 + value: MouseDrag.posStiffness + decimals: decimalPlaces + stepSize: step + onValueChanged: MouseDrag.posStiffness = posStiffness.value + } + + Switch { + Layout.columnSpan: 8 + objectName: "switchCOM" + text: qsTr("Apply force to center of mass") + onToggled: { + MouseDrag.OnSwitchCOM(checked); + } + } +} diff --git a/src/gui/plugins/mouse_drag/MouseDrag.qrc b/src/gui/plugins/mouse_drag/MouseDrag.qrc new file mode 100644 index 0000000000..89e46ce6cb --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.qrc @@ -0,0 +1,5 @@ + + + MouseDrag.qml + + diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index e95eeadc47..6912bc8942 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -173,6 +173,9 @@ namespace gz::sim /// \brief Block orbit public: bool blockOrbit = false; + + /// \brief True if BlockOrbit events should be sent + public: bool sendBlockOrbit = false; }; } @@ -499,10 +502,17 @@ void TransformControlPrivate::HandleTransform() this->HandleMouseEvents(); - gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &blockOrbitEvent); + if (this->sendBlockOrbit) + { + // Events with false should only be sent once + if (!this->blockOrbit) + this->sendBlockOrbit = false; + + gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &blockOrbitEvent); + } } ///////////////////////////////////////////////// @@ -534,6 +544,7 @@ void TransformControlPrivate::HandleMouseEvents() if (axis != gz::math::Vector3d::Zero) { this->blockOrbit = true; + this->sendBlockOrbit = true; // start the transform process this->transformControl.SetActiveAxis(axis); this->transformControl.Start(); @@ -553,6 +564,7 @@ void TransformControlPrivate::HandleMouseEvents() else { this->blockOrbit = false; + this->sendBlockOrbit = true; return; } } @@ -560,6 +572,7 @@ void TransformControlPrivate::HandleMouseEvents() else if (this->mouseEvent.Type() == gz::common::MouseEvent::RELEASE) { this->blockOrbit = false; + this->sendBlockOrbit = true; this->isStartWorldPosSet = false; if (this->transformControl.Active()) @@ -706,6 +719,7 @@ void TransformControlPrivate::HandleMouseEvents() } this->blockOrbit = true; + this->sendBlockOrbit = true; // compute the the start and end mouse positions in normalized coordinates auto imageWidth = static_cast(this->camera->ImageWidth()); auto imageHeight = static_cast( diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index b65f43dd45..25dd945633 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -180,7 +180,7 @@ void VisualizeLidar::LoadLidar() return; } - if (!scene->IsInitialized() || scene->VisualCount() == 0) + if (!scene->IsInitialized()) { return; } diff --git a/src/gz.cc b/src/gz.cc index 7d355424e0..3c7772e627 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -33,6 +33,7 @@ #include #include "gz/sim/config.hh" +#include "gz/sim/InstallationDirectories.hh" #include "gz/sim/Server.hh" #include "gz/sim/ServerConfig.hh" @@ -70,7 +71,8 @@ extern "C" void cmdVerbosity( ////////////////////////////////////////////////// extern "C" const char *worldInstallDir() { - return GZ_SIM_WORLD_INSTALL_DIR; + static std::string worldInstallDir = gz::sim::getWorldInstallDir(); + return worldInstallDir.c_str(); } ////////////////////////////////////////////////// diff --git a/src/rendering/CMakeLists.txt b/src/rendering/CMakeLists.txt index a8a510694f..2b551f9da2 100644 --- a/src/rendering/CMakeLists.txt +++ b/src/rendering/CMakeLists.txt @@ -2,6 +2,7 @@ set (rendering_comp_sources MarkerManager.cc RenderUtil.cc SceneManager.cc + WrenchVisualizer.cc ) if (MSVC) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 49ed8835f5..475e099903 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1271,7 +1271,7 @@ void RenderUtil::Update() } else { - ignerr << "Failed to create light" << std::endl; + gzerr << "Failed to create light" << std::endl; } } diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index acfdbb1232..4cef12fde0 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -702,23 +702,22 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, } else if (_geom.Type() == sdf::GeometryType::MESH) { - auto fullPath = asFullPath(_geom.MeshShape()->Uri(), - _geom.MeshShape()->FilePath()); - if (fullPath.empty()) - { - gzerr << "Mesh geometry missing uri" << std::endl; - return geom; - } rendering::MeshDescriptor descriptor; - - // Assume absolute path to mesh file - descriptor.meshName = fullPath; + descriptor.mesh = loadMesh(*_geom.MeshShape()); + if (!descriptor.mesh) + return geom; + std::string meshUri = + (common::URI(_geom.MeshShape()->Uri()).Scheme() == "name") ? + common::basename(_geom.MeshShape()->Uri()) : + asFullPath(_geom.MeshShape()->Uri(), + _geom.MeshShape()->FilePath()); + auto a = asFullPath(_geom.MeshShape()->Uri(), + _geom.MeshShape()->FilePath()); + + descriptor.meshName = meshUri; descriptor.subMeshName = _geom.MeshShape()->Submesh(); descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh(); - common::MeshManager *meshManager = - common::MeshManager::Instance(); - descriptor.mesh = meshManager->Load(descriptor.meshName); geom = this->dataPtr->scene->CreateMesh(descriptor); scale = _geom.MeshShape()->Scale(); } @@ -1191,7 +1190,7 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, if (this->HasEntity(_id)) { - ignerr << "Light with Id: [" << _id << "] can not be create there is " + gzerr << "Light with Id: [" << _id << "] can not be create there is " "another entity with the same entity number" << std::endl; return nullptr; } @@ -2373,8 +2372,8 @@ SceneManager::LoadAnimations(const sdf::Actor &_actor) { if (!meshSkel->AddBvhAnimation(animFilename, animScale)) { - ignerr << "Bvh animation in file " << animFilename - << " failed to load during actor creation" << std::endl; + gzerr << "Bvh animation in file " << animFilename + << " failed to load during actor creation" << std::endl; continue; } } @@ -2470,8 +2469,8 @@ SceneManagerPrivate::LoadTrajectories(const sdf::Actor &_actor, const sdf::Trajectory *trajSdf = _actor.TrajectoryByIndex(i); if (nullptr == trajSdf) { - ignerr << "Null trajectory SDF for [" << _actor.Name() << "]" - << std::endl; + gzerr << "Null trajectory SDF for [" << _actor.Name() << "]" + << std::endl; continue; } common::TrajectoryInfo trajInfo; @@ -2514,9 +2513,9 @@ SceneManagerPrivate::LoadTrajectories(const sdf::Actor &_actor, } else { - ignwarn << "Animation has no x displacement. " - << "Ignoring for the animation in '" - << animation->Filename() << "'." << std::endl; + gzwarn << "Animation has no x displacement. " + << "Ignoring for the animation in '" + << animation->Filename() << "'." << std::endl; } animInterpolateCheck.insert(animation->Filename()); } diff --git a/src/rendering/WrenchVisualizer.cc b/src/rendering/WrenchVisualizer.cc new file mode 100644 index 0000000000..0adab74c16 --- /dev/null +++ b/src/rendering/WrenchVisualizer.cc @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2023 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. + * +*/ + +#include +#include +#include +#include +#include + +#include "gz/sim/rendering/WrenchVisualizer.hh" + +using namespace gz; +using namespace sim; +using namespace detail; + +/// Private data for the WrenchVisualizer class +class gz::sim::detail::WrenchVisualizer::Implementation +{ + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; +}; + +///////////////////////////////////////////////// +WrenchVisualizer::WrenchVisualizer() : + dataPtr(utils::MakeUniqueImpl()) +{ +} + +///////////////////////////////////////////////// +WrenchVisualizer::~WrenchVisualizer() = default; + +///////////////////////////////////////////////// +bool WrenchVisualizer::Init(rendering::ScenePtr _scene) +{ + if (!_scene) + { + return false; + } + this->dataPtr->scene = _scene; + return true; +} + +///////////////////////////////////////////////// +rendering::ArrowVisualPtr WrenchVisualizer::CreateForceVisual( + rendering::MaterialPtr _material) +{ + if (!this->dataPtr->scene) + { + gzwarn << "WrenchVisualizer not initialized" << std::endl; + return rendering::ArrowVisualPtr(); + } + rendering::ArrowVisualPtr forceVisual = + this->dataPtr->scene->CreateArrowVisual(); + forceVisual->SetMaterial(_material); + forceVisual->ShowArrowHead(true); + forceVisual->ShowArrowShaft(true); + forceVisual->ShowArrowRotation(false); + return forceVisual; +} + +///////////////////////////////////////////////// +rendering::VisualPtr WrenchVisualizer::CreateTorqueVisual( + rendering::MaterialPtr _material) +{ + if (!this->dataPtr->scene) + { + gzwarn << "WrenchVisualizer not initialized" << std::endl; + return rendering::ArrowVisualPtr(); + } + auto meshMgr = common::MeshManager::Instance(); + std::string meshName{"torque_tube"}; + if (!meshMgr->HasMesh(meshName)) + meshMgr->CreateTube(meshName, 0.28f, 0.3f, 0.2f, 1, 32); + auto torqueTube = this->dataPtr->scene->CreateVisual(); + torqueTube->AddGeometry(this->dataPtr->scene->CreateMesh(meshName)); + torqueTube->SetOrigin(0, 0, -0.9f); + torqueTube->SetLocalPosition(0, 0, 0); + + auto cylinder = this->dataPtr->scene->CreateVisual(); + cylinder->AddGeometry(this->dataPtr->scene->CreateCylinder()); + cylinder->SetOrigin(0, 0, -0.5); + cylinder->SetLocalScale(0.01, 0.01, 0.8); + cylinder->SetLocalPosition(0, 0, 0); + + auto torqueVisual = this->dataPtr->scene->CreateVisual(); + torqueVisual->AddChild(torqueTube); + torqueVisual->AddChild(cylinder); + torqueVisual->SetMaterial(_material); + return torqueVisual; +} + +///////////////////////////////////////////////// +void WrenchVisualizer::UpdateVectorVisual(rendering::VisualPtr _visual, + const math::Vector3d &_direction, + const math::Vector3d &_position, + const double _size, + const bool _tip) +{ + math::Quaterniond quat; + quat.SetFrom2Axes(math::Vector3d::UnitZ, _direction); + if (_tip) + { + _visual->SetLocalPosition( + _position - 0.75 * _size * _direction.Normalized()); + } + else + { + _visual->SetLocalPosition(_position); + } + _visual->SetLocalRotation(quat); + _visual->SetLocalScale(_size); +} diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 06b82f98f8..eadb2f47fe 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -81,15 +81,16 @@ class gz::sim::systems::ApplyLinkWrenchPrivate /// it's a model, its canonical link is returned. /// \param[out] Force to apply. /// \param[out] Torque to apply. +/// \param[out] Offset of the force application point expressed in the link +/// frame. /// \return Target link entity. Link decomposeMessage(const EntityComponentManager &_ecm, const msgs::EntityWrench &_msg, math::Vector3d &_force, - math::Vector3d &_torque) + math::Vector3d &_torque, math::Vector3d &_offset) { if (_msg.wrench().has_force_offset()) { - gzwarn << "Force offset currently not supported, it will be ignored." - << std::endl; + _offset = msgs::Convert(_msg.wrench().force_offset()); } if (_msg.wrench().has_force()) @@ -270,8 +271,9 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, auto msg = this->dataPtr->newWrenches.front(); math::Vector3d force; + math::Vector3d offset; math::Vector3d torque; - auto link = decomposeMessage(_ecm, msg, force, torque); + auto link = decomposeMessage(_ecm, msg, force, torque, offset); if (!link.Valid(_ecm)) { gzerr << "Entity not found." << std::endl @@ -280,11 +282,12 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, continue; } - link.AddWorldWrench(_ecm, force, torque); + link.AddWorldWrench(_ecm, force, torque, offset); if (this->dataPtr->verbose) { - gzdbg << "Applying wrench [" << force << " " << torque << "] to entity [" + gzdbg << "Applying wrench [" << force << " " << torque + << "] with force offset [" << offset << "] to entity [" << link.Entity() << "] for 1 time step." << std::endl; } @@ -295,15 +298,16 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, for (auto msg : this->dataPtr->persistentWrenches) { math::Vector3d force; + math::Vector3d offset; math::Vector3d torque; - auto link = decomposeMessage(_ecm, msg, force, torque); + auto link = decomposeMessage(_ecm, msg, force, torque, offset); if (!link.Valid(_ecm)) { // Not an error, persistent wrenches can be applied preemptively before // an entity is inserted continue; } - link.AddWorldWrench(_ecm, force, torque); + link.AddWorldWrench(_ecm, force, torque, offset); } } diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.hh b/src/systems/apply_link_wrench/ApplyLinkWrench.hh index 625db47578..f2c449e51f 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.hh +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.hh @@ -38,6 +38,9 @@ namespace systems /// that will receive a wrench. If a model is provided, its canonical link /// will receive it. No other entity types are supported. /// + /// Forces and torques are expressed in world coordinates, and the force + /// offset is expressed in the link frame. + /// /// ## Topics /// /// * /world//wrench diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index dc1b0ced76..357b01df97 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -394,8 +394,8 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, std::bind(&LinearBatteryPluginPrivate::OnBatteryStopDrainingMsg, this->dataPtr.get(), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - ignmsg << "LinearBatteryPlugin subscribes to stop power draining topic [" - << topic << "]." << std::endl; + gzmsg << "LinearBatteryPlugin subscribes to stop power draining topic [" + << topic << "]." << std::endl; sdfElem = sdfElem->GetNextElement("power_draining_topic"); } } @@ -498,7 +498,7 @@ void LinearBatteryPlugin::PreUpdate( bool success = this->dataPtr->battery->SetPowerLoad( this->dataPtr->consumerId, total_power_load); if (!success) - ignerr << "Failed to set consumer power load." << std::endl; + gzerr << "Failed to set consumer power load." << std::endl; // Start draining the battery if the robot has started moving if (!this->dataPtr->startDraining) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index ef4da0a4dd..6f44922062 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -643,7 +643,7 @@ void Buoyancy::PostUpdate( bool Buoyancy::IsEnabled(Entity _entity, const EntityComponentManager &_ecm) const { - return this->IsEnabled(_entity, _ecm); + return this->dataPtr->IsEnabled(_entity, _ecm); } GZ_ADD_PLUGIN(Buoyancy, diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index fe5b57f940..5c678aa2db 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -134,7 +134,7 @@ void EnvironmentPreload::PreUpdate( } else if (unitName != "radians") { - ignerr << "Unrecognized unit " << unitName << "\n"; + gzerr << "Unrecognized unit " << unitName << "\n"; } } } diff --git a/src/systems/environmental_sensor_system/CMakeLists.txt b/src/systems/environmental_sensor_system/CMakeLists.txt index c589a464bf..ff91c6e175 100644 --- a/src/systems/environmental_sensor_system/CMakeLists.txt +++ b/src/systems/environmental_sensor_system/CMakeLists.txt @@ -12,4 +12,6 @@ gz_build_tests(TYPE UNIT TransformTypes_TEST.cc LIB_DEPS gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} + ENVIRONMENT + GZ_SIM_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 6b2167a072..8e53087ab7 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -295,7 +295,7 @@ void Hydrodynamics::Configure( { if (_sdf->HasElement("waterDensity")) { - ignwarn << + gzwarn << " parameter is deprecated and will be removed Ignition G.\n" << "\tPlease update your SDF to use instead."; } @@ -342,7 +342,7 @@ void Hydrodynamics::Configure( if (warnBehaviourChange) { - ignwarn << "You are using parameters that may cause instabilities " + gzwarn << "You are using parameters that may cause instabilities " << "in your simulation. If your simulation crashes we recommend " << "renaming -> and likewise for other axis " << "for more information see:" << std::endl diff --git a/src/systems/logical_audio_sensor_plugin/CMakeLists.txt b/src/systems/logical_audio_sensor_plugin/CMakeLists.txt index 607c0705d0..9bd43e073e 100644 --- a/src/systems/logical_audio_sensor_plugin/CMakeLists.txt +++ b/src/systems/logical_audio_sensor_plugin/CMakeLists.txt @@ -18,4 +18,6 @@ gz_build_tests(TYPE UNIT ${gtest_sources} LIB_DEPS ${PROJECT_LIBRARY_TARGET_NAME}-logicalaudiosensorplugin-system + ENVIRONMENT + GZ_SIM_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.hh b/src/systems/multicopter_control/MulticopterVelocityControl.hh index 2c1f4b37af..c414d1ec53 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.hh +++ b/src/systems/multicopter_control/MulticopterVelocityControl.hh @@ -21,6 +21,9 @@ #include #include +#include +#include +#include #include #include diff --git a/src/systems/performer_detector/PerformerDetector.hh b/src/systems/performer_detector/PerformerDetector.hh index 84c8e12d9c..bbdb9e3ddf 100644 --- a/src/systems/performer_detector/PerformerDetector.hh +++ b/src/systems/performer_detector/PerformerDetector.hh @@ -23,6 +23,7 @@ #include #include +#include #include #include "gz/sim/Model.hh" diff --git a/src/systems/physics/CMakeLists.txt b/src/systems/physics/CMakeLists.txt index ad18e3f494..006fe39fa4 100644 --- a/src/systems/physics/CMakeLists.txt +++ b/src/systems/physics/CMakeLists.txt @@ -27,4 +27,6 @@ gz_build_tests(TYPE UNIT ${gtest_sources} LIB_DEPS gz-physics${GZ_PHYSICS_VER}::core + ENVIRONMENT + GZ_SIM_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 27fb5378f9..59cd31bc33 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -1391,15 +1392,9 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm, return true; } - auto &meshManager = *common::MeshManager::Instance(); - auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath()); - auto *mesh = meshManager.Load(fullPath); - if (nullptr == mesh) - { - gzwarn << "Failed to load mesh from [" << fullPath - << "]." << std::endl; + const common::Mesh *mesh = loadMesh(*meshSdf); + if (!mesh) return true; - } auto linkMeshFeature = this->entityLinkMap.EntityCast(_parent->Data()); @@ -3733,12 +3728,21 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) return; } + // Using ExtraContactData to expose contact Norm, Force & Depth + using Policy = physics::FeaturePolicy3d; + using GCFeature = physics::GetContactsFromLastStepFeature; + using ExtraContactData = GCFeature::ExtraContactDataT; + + // A contact is described by a contactPoint and the corresponding + // extraContactData which we bundle in a pair data structure + using ContactData = std::pair; // 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 + // position and extra contact date (wrench, normal and penetration depth). + // This map groups contacts so that it is easy to query all the // contacts of one entity. - using EntityContactMap = std::unordered_map>; + using EntityContactMap = std::unordered_map>; // This data structure is essentially a mapping between a pair of entities and // a list of pointers to their contact object. We use a map inside a map to @@ -3752,16 +3756,19 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contactComposite : allContacts) { - const auto &contact = contactComposite.Get(); - auto coll1Entity = - this->entityCollisionMap.GetByPhysicsId(contact.collision1->EntityID()); - auto coll2Entity = - this->entityCollisionMap.GetByPhysicsId(contact.collision2->EntityID()); + const auto &contactPoint = + contactComposite.Get(); + const auto &extraContactData = contactComposite.Query(); + auto coll1Entity = this->entityCollisionMap.GetByPhysicsId( + contactPoint.collision1->EntityID()); + auto coll2Entity = this->entityCollisionMap.GetByPhysicsId( + contactPoint.collision2->EntityID()); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) { - entityContactMap[coll1Entity][coll2Entity].push_back(&contact); - entityContactMap[coll2Entity][coll1Entity].push_back(&contact); + ContactData data = std::make_pair(&contactPoint, extraContactData); + entityContactMap[coll1Entity][coll2Entity].push_back(data); + entityContactMap[coll2Entity][coll1Entity].push_back(data); } } @@ -3802,9 +3809,36 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contact : contactData) { auto *position = contactMsg->add_position(); - position->set_x(contact->point.x()); - position->set_y(contact->point.y()); - position->set_z(contact->point.z()); + position->set_x(contact.first->point.x()); + position->set_y(contact.first->point.y()); + position->set_z(contact.first->point.z()); + + // Check if the extra contact data exists, + // since not all physics engines support it. + // Then, fill the msg with extra data. + if(contact.second != nullptr) + { + auto *normal = contactMsg->add_normal(); + normal->set_x(contact.second->normal.x()); + normal->set_y(contact.second->normal.y()); + normal->set_z(contact.second->normal.z()); + + auto *wrench = contactMsg->add_wrench(); + auto *body1Wrench = wrench->mutable_body_1_wrench(); + auto *body1Force = body1Wrench->mutable_force(); + body1Force->set_x(contact.second->force.x()); + body1Force->set_y(contact.second->force.y()); + body1Force->set_z(contact.second->force.z()); + + // The force on the second body is equal and opposite + auto *body2Wrench = wrench->mutable_body_2_wrench(); + auto *body2Force = body2Wrench->mutable_force(); + body2Force->set_x(-contact.second->force.x()); + body2Force->set_y(-contact.second->force.y()); + body2Force->set_z(-contact.second->force.z()); + + contactMsg->add_depth(contact.second->depth); + } } } diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index a30bcf319e..c9e81a4b76 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -281,12 +281,15 @@ void SensorsPrivate::RunOnce() { { std::unique_lock cvLock(this->renderMutex); - this->renderCv.wait(cvLock, [this]() + this->renderCv.wait_for(cvLock, std::chrono::microseconds(1000), [this]() { return !this->running || this->updateAvailable; }); } + if (!this->updateAvailable) + return; + if (!this->running) return; @@ -640,7 +643,7 @@ void Sensors::Update(const UpdateInfo &_info, _ecm.HasComponentType(components::WideAngleCamera::typeId))) { std::unique_lock lock(this->dataPtr->renderMutex); - igndbg << "Initialization needed" << std::endl; + gzdbg << "Initialization needed" << std::endl; this->dataPtr->renderUtil.Init(); this->dataPtr->nextUpdateTime = _info.simTime; } diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index b911a5674a..e7663924b2 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -14,6 +14,8 @@ * limitations under the License. * */ +#include +#include #include #include #include @@ -144,6 +146,18 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Linear velocity of the vehicle. public: double linearVelocity = 0.0; + /// \brief deadband in newtons + public: double deadband = 0.0; + + /// \brief Flag to enable/disable deadband + public: bool enableDeadband = false; + + /// \brief Mutex to protect enableDeadband + public: std::mutex deadbandMutex; + + /// \brief Topic name used to enable/disable the deadband + public: std::string deadbandTopic = ""; + /// \brief Topic name used to control thrust. Optional public: std::string topic = ""; @@ -159,6 +173,11 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Callback for handling thrust update public: void OnCmdThrust(const msgs::Double &_msg); + /// \brief Callback for handling deadband enable/disable update + /// \param[in] _msg boolean msg to indicate whether to enable or disable + /// the deadband + public: void OnDeadbandEnable(const msgs::Boolean &_msg); + /// \brief Recalculates and updates the thrust coefficient. public: void UpdateThrustCoefficient(); @@ -179,6 +198,12 @@ class gz::sim::systems::ThrusterPrivateData /// \return True if battery is charged, false otherwise. If no battery found, /// returns true. public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const; + + /// \brief Applies the deadband to the thrust and angular velocity by setting + /// those values to zero if the thrust absolute value is below the deadband + /// \param[in] _thrust thrust in N used for check + /// \param[in] _angvel angular velocity in rad/s + public: void ApplyDeadband(double &_thrust, double &_angVel); }; ///////////////////////////////////////////////// @@ -277,6 +302,13 @@ void Thruster::Configure( } } + // Get deadband, default to 0 + if (_sdf->HasElement("deadband")) + { + this->dataPtr->deadband = _sdf->Get("deadband"); + this->dataPtr->enableDeadband = true; + } + // Get a custom topic. if (_sdf->HasElement("topic")) { @@ -312,6 +344,8 @@ void Thruster::Configure( // Subscribe to specified topic for force commands thrusterTopic = gz::transport::TopicUtils::AsValidTopic( ns + "/" + this->dataPtr->topic); + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( + ns + "/" + this->dataPtr->topic + "/enable_deadband"); if (this->dataPtr->opmode == ThrusterPrivateData::OperationMode::ForceCmd) { this->dataPtr->node.Subscribe( @@ -347,6 +381,9 @@ void Thruster::Configure( feedbackTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/ang_vel"); + + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/enable_deadband"); } else { @@ -362,11 +399,24 @@ void Thruster::Configure( feedbackTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/force"); + + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/enable_deadband"); } gzmsg << "Thruster listening to commands on [" << thrusterTopic << "]" << std::endl; + if (!this->dataPtr->deadbandTopic.empty()) + { + this->dataPtr->node.Subscribe( + this->dataPtr->deadbandTopic, + &ThrusterPrivateData::OnDeadbandEnable, + this->dataPtr.get()); + gzmsg << "Thruster listening to enable_deadband on [" + << this->dataPtr->deadbandTopic << "]" << std::endl; + } + this->dataPtr->pub = this->dataPtr->node.Advertise( feedbackTopic); @@ -451,7 +501,7 @@ void Thruster::Configure( { if (!_sdf->HasElement("battery_name")) { - ignerr << "Specified a but missing ." + gzerr << "Specified a but missing ." "Specify a battery name so the power load can be assigned to it." << std::endl; } @@ -475,6 +525,26 @@ void ThrusterPrivateData::OnCmdThrust(const gz::msgs::Double &_msg) this->propellerAngVel = this->ThrustToAngularVec(this->thrust); } +///////////////////////////////////////////////// +void ThrusterPrivateData::OnDeadbandEnable(const gz::msgs::Boolean &_msg) +{ + std::lock_guard lock(this->deadbandMutex); + if (_msg.data() != this->enableDeadband) + { + if (_msg.data()) + { + gzmsg << "Enabling deadband." << std::endl; + } + else + { + gzmsg << "Disabling deadband." << std::endl; + } + + this->enableDeadband = _msg.data(); + } + +} + ///////////////////////////////////////////////// void ThrusterPrivateData::OnCmdAngVel(const gz::msgs::Double &_msg) { @@ -550,6 +620,16 @@ bool ThrusterPrivateData::HasSufficientBattery( return result; } +///////////////////////////////////////////////// +void ThrusterPrivateData::ApplyDeadband(double &_thrust, double &_angVel) +{ + if (abs(_thrust) < this->deadband) + { + _thrust = 0.0; + _angVel = 0.0; + } +} + ///////////////////////////////////////////////// void Thruster::PreUpdate( const gz::sim::UpdateInfo &_info, @@ -562,6 +642,9 @@ void Thruster::PreUpdate( { return; } + if (!_ecm.HasEntity(this->dataPtr->linkEntity)){ + return; + } // Init battery consumption if it was set if (!this->dataPtr->batteryName.empty() && @@ -586,17 +669,17 @@ void Thruster::PreUpdate( }); if (numBatteriesWithName == 0) { - ignerr << "Can't assign battery consumption to battery: [" - << this->dataPtr->batteryName << "]. No batteries" - "were found with the given name." << std::endl; + gzerr << "Can't assign battery consumption to battery: [" + << this->dataPtr->batteryName << "]. No batteries" + "were found with the given name." << std::endl; return; } if (numBatteriesWithName > 1) { - ignerr << "More than one battery found with name: [" - << this->dataPtr->batteryName << "]. Please make" - "sure battery names are unique within the system." - << std::endl; + gzerr << "More than one battery found with name: [" + << this->dataPtr->batteryName << "]. Please make" + "sure battery names are unique within the system." + << std::endl; return; } @@ -631,6 +714,15 @@ void Thruster::PreUpdate( desiredPropellerAngVel = this->dataPtr->propellerAngVel; } + { + std::lock_guard lock(this->dataPtr->deadbandMutex); + if (this->dataPtr->enableDeadband) + { + this->dataPtr->ApplyDeadband(desiredThrust, desiredPropellerAngVel); + } + } + + msgs::Double angvel; // PID control double torque = 0.0; diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 81cbb12e94..5c9cdab562 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -85,6 +85,11 @@ namespace systems /// [Optional, defaults to 1000N or 1000rad/s] /// - - Minimum input thrust or angular velocity command. /// [Optional, defaults to -1000N or -1000rad/s] + /// - - Deadband of the thruster. Absolute value below which the + /// thruster won't spin nor generate thrust. This value can + /// be changed at runtime using a topic. The topic is either + /// `/model/{ns}/joint/{jointName}/enable_deadband` or + /// `{ns}/{topic}/enable_deadband` depending on other params /// - - Relative speed reduction between the water /// at the propeller (Va) vs behind the vessel. /// [Optional, defults to 0.2] diff --git a/src/systems/touch_plugin/TouchPlugin.cc b/src/systems/touch_plugin/TouchPlugin.cc index 19a325d9b9..9dec83362b 100644 --- a/src/systems/touch_plugin/TouchPlugin.cc +++ b/src/systems/touch_plugin/TouchPlugin.cc @@ -103,7 +103,7 @@ class gz::sim::systems::TouchPluginPrivate public: bool initialized{false}; /// \brief Set during Load to true if the configuration for the plugin is - /// valid and the post update can run + /// valid and the pre and post update can run public: bool validConfig{false}; /// \brief Whether the plugin is enabled. @@ -373,7 +373,7 @@ void TouchPlugin::Configure(const Entity &_entity, void TouchPlugin::PreUpdate(const UpdateInfo &, EntityComponentManager &_ecm) { GZ_PROFILE("TouchPlugin::PreUpdate"); - if (!this->dataPtr->initialized) + if ((!this->dataPtr->initialized) && this->dataPtr->sdfConfig) { // We call Load here instead of Configure because we can't be guaranteed // that all entities have been created when Configure is called @@ -381,9 +381,8 @@ void TouchPlugin::PreUpdate(const UpdateInfo &, EntityComponentManager &_ecm) this->dataPtr->initialized = true; } - // This is not an "else" because "initialized" can be set in the if block - // above - if (this->dataPtr->initialized) + // If Load() was successful, validConfig is set to true + if (this->dataPtr->validConfig) { // Update target entities when new collisions are added std::vector potentialEntities; diff --git a/src/systems/touch_plugin/TouchPlugin.hh b/src/systems/touch_plugin/TouchPlugin.hh index 189bcdbc75..27a4d848aa 100644 --- a/src/systems/touch_plugin/TouchPlugin.hh +++ b/src/systems/touch_plugin/TouchPlugin.hh @@ -32,21 +32,25 @@ namespace systems // Forward declaration class TouchPluginPrivate; - /// \brief Plugin which checks if a model has touched some specific target - /// for a given time continuously and exclusively. After the touch is - /// completed, the plugin is disabled. It can be re-enabled through an - /// Gazebo Transport service. + /// \brief Plugin which publishes a message if the model it is attached + /// to has touched one or more specified targets continuously during a + /// given time. /// - /// It requires that contact sensors be placed in at least one link on the - /// model on which this plugin is attached. + /// After publishing, the plugin is disabled. It can be re-enabled through + /// a Gazebo Transport service call. + /// + /// The plugin requires that a contact sensors is placed in at least one + /// link on the model on which this plugin is attached. /// /// Parameters: /// - /// - `` Scoped name of the desired collision entity that is checked - /// to see if it's touching this model. This can be a substring - /// of the desired collision name so we match more than one - /// collision. For example, using the name of a model will match - /// all its collisions. + /// - `` Name, or substring of a name, that identifies the target + /// collision entity/entities. + /// This value is searched in the scoped name of all collision + /// entities, so it can possibly match more than one collision. + /// For example, using the name of a model will match all of its + /// collisions (scoped name + /// /model_name/link_name/collision_name). /// /// - `