diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index b670b9997..25021c47a 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -1,10 +1,10 @@ libbenchmark-dev libeigen3-dev -libignition-cmake3-dev -libignition-common5-dev -libignition-math7-dev -libignition-math7-eigen3-dev -libignition-plugin2-dev -libignition-utils2-cli-dev -libignition-utils2-dev +libgz-cmake3-dev +libgz-common5-dev +libgz-math7-dev +libgz-math7-eigen3-dev +libgz-plugin2-dev +libgz-utils2-cli-dev +libgz-utils2-dev libsdformat13-dev diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ba1eed61a..bd14ed689 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -11,7 +11,7 @@ jobs: uses: actions/checkout@v2 - name: Compile and test id: ci - uses: ignition-tooling/action-ignition-ci@focal + uses: gazebo-tooling/action-gz-ci@focal with: codecov-enabled: true cppcheck-enabled: true @@ -24,4 +24,4 @@ jobs: uses: actions/checkout@v2 - name: Compile and test id: ci - uses: ignition-tooling/action-ignition-ci@jammy + uses: gazebo-tooling/action-gz-ci@jammy diff --git a/.github/workflows/pr-collection-labeler.yml b/.github/workflows/pr-collection-labeler.yml index 7d7b4e179..38c4fc13b 100644 --- a/.github/workflows/pr-collection-labeler.yml +++ b/.github/workflows/pr-collection-labeler.yml @@ -8,6 +8,6 @@ jobs: steps: - name: Add collection labels if: github.event.action == 'opened' - uses: ignition-tooling/pr-collection-labeler@v1 + uses: gazebo-tooling/pr-collection-labeler@v1 with: github-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/CMakeLists.txt b/CMakeLists.txt index d3dc8582d..f03588387 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,9 +16,7 @@ find_package(gz-cmake3 REQUIRED) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -gz_configure_project( - REPLACE_IGNITION_INCLUDE_PATH gz/physics - VERSION_SUFFIX pre1) +gz_configure_project(VERSION_SUFFIX pre1) #============================================================================ # Set project-specific options @@ -96,7 +94,7 @@ set(GZ_PHYSICS_RESOURCE_DIR "${CMAKE_SOURCE_DIR}/resources") # Plugin install dirs set(GZ_PHYSICS_ENGINE_INSTALL_DIR - ${CMAKE_INSTALL_PREFIX}/${IGN_LIB_INSTALL_DIR}/gz-${IGN_DESIGNATION}-${PROJECT_VERSION_MAJOR}/engine-plugins + ${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/engine-plugins ) #============================================================================ diff --git a/Changelog.md b/Changelog.md index 2043ed575..a2154cae1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -9,7 +9,7 @@ ### Gazebo Physics 5.1.0 (2021-11-12) -1. Remove unused ign_auto_headers.hh.in +1. Remove unused gz_auto_headers.hh.in * [Pull request #305](https://github.com/gazebosim/gz-physics/pull/305) 1. Added DART feature for setting joint limits dynamically. @@ -38,7 +38,7 @@ 1. Fix TPE Link velocity not being updated and Model velocity not having any effect. * [Pull request #289](https://github.com/gazebosim/gz-physics/pull/289) -1. Make ignition-physics CMake config files relocatable +1. Make gz-physics CMake config files relocatable * [Pull request #282](https://github.com/gazebosim/gz-physics/pull/282) 1. Added DART feature for setting joint limits dynamically. @@ -69,7 +69,7 @@ ### Gazebo Physics 4.3.0 (2021-11-11) -1. Remove unused ign_auto_headers.hh.in +1. Remove unused gz_auto_headers.hh.in * [Pull request #305](https://github.com/gazebosim/gz-physics/pull/305) 1. Added DART feature for setting joint limits dynamically. @@ -91,7 +91,7 @@ * [Pull request #287](https://github.com/gazebosim/gz-physics/pull/287) * [Pull request #281](https://github.com/gazebosim/gz-physics/pull/281) -1. Make ignition-physics CMake config files relocatable +1. Make gz-physics CMake config files relocatable * [Pull request #282](https://github.com/gazebosim/gz-physics/pull/282) ### Gazebo Physics 4.2.0 (2021-07-16) @@ -323,7 +323,7 @@ ### Gazebo Physics 2.5.0 (2021-11-09) -1. Remove unused ign_auto_headers.hh.in +1. Remove unused gz_auto_headers.hh.in * [Pull request #305](https://github.com/gazebosim/gz-physics/pull/305) 1. Added DART feature for setting joint limits dynamically. diff --git a/Migration.md b/Migration.md index 65fea3e8c..bbf3b5979 100644 --- a/Migration.md +++ b/Migration.md @@ -61,7 +61,7 @@ release will remove the deprecated code. ### Modifications -1. Depends on ignition-utils1. +1. Depends on gz-utils1. 1. Depends on sdformat11. diff --git a/README.md b/README.md index 196e80b6a..97c5fde85 100644 --- a/README.md +++ b/README.md @@ -128,12 +128,12 @@ Follow these steps to run tests and static code analysis in your clone of this r Refer to the following table for information about important directories and files in this repository. ``` -ign-physics +gz-physics ├── bullet Files for bullet plugin component. ├── dartsim Files for dartsim plugin component. ├── example Examples about how to use the library ├── heightmap Heightmap related header files. -├── include/ignition/physics Header files. +├── include/gz/physics Header files. ├── mesh Files for mesh component. ├── resources Model and mesh resource files used by tests. ├── sdf Files for sdf component. diff --git a/api.md.in b/api.md.in index 9bf3d8a27..e9222f8b3 100644 --- a/api.md.in +++ b/api.md.in @@ -1,6 +1,6 @@ -## Gazebo @IGN_DESIGNATION_CAP@ +## Gazebo @GZ_DESIGNATION_CAP@ -Gazebo @IGN_DESIGNATION_CAP@ is a component in Gazebo, a set of libraries +Gazebo @GZ_DESIGNATION_CAP@ is a component in Gazebo, a set of libraries designed to rapidly develop robot and simulation applications. ## License diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 13bacdd49..83a515e47 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -206,6 +206,32 @@ inline Eigen::Isometry3d convert(const btTransform& tf) return output; } +inline btTransform GetWorldTransformOfLinkInertiaFrame( + const btMultiBody &body, + const std::size_t linkIndexInModel) +{ + const auto p = body.localPosToWorld( + linkIndexInModel, btVector3(0, 0, 0)); + const auto rot = body.localFrameToWorld( + linkIndexInModel, btMatrix3x3::getIdentity()); + return btTransform(rot, p); +} + +inline Eigen::Isometry3d GetWorldTransformOfLink( + const ModelInfo &model, + const LinkInfo &linkInfo) +{ + const auto &body = *model.body; + const auto indexOpt = linkInfo.indexInModel; + if (indexOpt.has_value()) + { + return convert(GetWorldTransformOfLinkInertiaFrame(body, *indexOpt)) + * linkInfo.inertiaToLinkFrame; + } + + return convert(body.getBaseWorldTransform()) * model.baseInertiaToLinkFrame; +} + class Base : public Implements3d> { // Note: Entity ID 0 is reserved for the "engine" @@ -266,7 +292,8 @@ class Base : public Implements3d> { // We are adding the root link. This means the model should not already // have a root link - assert(model->linkEntityIds.empty()); + // This check makes `ConstructEmptyLink` to fail + // assert(model->linkEntityIds.empty()); } model->linkEntityIds.push_back(id); diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 7df61e2f1..0cf8f458d 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -225,6 +225,19 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) } world->world->removeMultiBody(model->body.get()); + for (const auto linkID : model->linkEntityIds) + { + const auto &link = this->links.at(linkID); + world->world->removeCollisionObject(link->collider.get()); + for (const auto shapeID : link->collisionEntityIds) + this->collisions.erase(shapeID); + + this->links.erase(linkID); + } + + for (const auto jointID : model->jointEntityIds) + this->joints.erase(jointID); + this->models.erase(_modelID); return true; } @@ -265,6 +278,137 @@ bool EntityManagementFeatures::RemoveModelByName( this->GenerateIdentity(it->second, this->models.at(it->second))); } + +///////////////////////////////////////////////// +const std::string &EntityManagementFeatures::GetEngineName( + const Identity &) const +{ + static const std::string engineName = "bullet-featherstone"; + return engineName; +} + +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetEngineIndex(const Identity &) const +{ + return 0; +} + +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetWorldCount( + const Identity &) const +{ + return worlds.size(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetWorld( + const Identity &, const std::size_t _requestedWorldIndex) const +{ + // _worldIndex is not the same as a WorldID. The value of _worldIndex should + // range from 0 to GetWorldCount()-1. The most efficient implementation + // would be to maintain a std::vector of WorldIDs, but then we'd have to + // manage that data when worlds are added and removed. + std::size_t currentWorldIndex = 0; + for (const auto &[worldID, world] : this->worlds) + { + if (currentWorldIndex == _requestedWorldIndex) + return this->GenerateIdentity(worldID, world); + } + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetWorld( + const Identity &, const std::string &_requestedWorldName) const +{ + // We could speed this up by maintaining a hashmap from world name to world ID + for (const auto &[worldID, world] : this->worlds) + { + if (world->name == _requestedWorldName) + return this->GenerateIdentity(worldID, world); + } + + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +const std::string &EntityManagementFeatures::GetWorldName( + const Identity &_worldID) const +{ + return this->ReferenceInterface(_worldID)->name; +} + +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetWorldIndex( + const Identity &) const +{ + return 0; +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetEngineOfWorld( + const Identity &) const +{ + return this->GenerateIdentity(0); +} + +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetModelCount( + const Identity &) const +{ + return this->models.size(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetModel( + const Identity &_worldID, std::size_t _modelIndex) const +{ + const auto *world = this->ReferenceInterface(_worldID); + const auto it = world->modelIndexToEntityId.find(_modelIndex); + if (it == world->modelIndexToEntityId.end()) + return this->GenerateInvalidId(); + + return this->GenerateIdentity(it->second, this->models.at(it->second)); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetModel( + const Identity &_worldID, const std::string &_modelName) const +{ + const auto *world = this->ReferenceInterface(_worldID); + const auto it = world->modelNameToEntityId.find(_modelName); + if (it == world->modelNameToEntityId.end()) + return this->GenerateInvalidId(); + + return this->GenerateIdentity(it->second, this->models.at(it->second)); +} + +///////////////////////////////////////////////// +const std::string &EntityManagementFeatures::GetModelName( + const Identity &_modelID) const +{ + return this->ReferenceInterface(_modelID)->name; +} + +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetModelIndex( + const Identity &_modelID) const +{ + // The root link does not have an index, so we give it an index of 0 and bump + // the rest up by one when providing an index to gazebo + const auto index = this->ReferenceInterface( + _modelID)->indexInWorld; + return index+1; +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetWorldOfModel( + const Identity &_modelID) const +{ + return this->ReferenceInterface(_modelID)->world; +} + } // namespace bullet_featherstone } // namespace physics } // namespace gz diff --git a/bullet-featherstone/src/EntityManagementFeatures.hh b/bullet-featherstone/src/EntityManagementFeatures.hh index 232e90cc1..4447e7118 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.hh +++ b/bullet-featherstone/src/EntityManagementFeatures.hh @@ -36,13 +36,61 @@ struct EntityManagementFeatureList : gz::physics::FeatureList< ConstructEmptyWorldFeature, GetLinkFromModel, GetJointFromModel, - GetShapeFromLink + GetShapeFromLink, + GetEngineInfo, + GetWorldFromEngine, + GetModelFromWorld > { }; class EntityManagementFeatures : public virtual Base, public virtual Implements3d { + // ----- GetEngineInfo ----- + public: const std::string &GetEngineName( + const Identity &_engineID) const override; + + public: std::size_t GetEngineIndex( + const Identity &_engineID) const override; + + // ----- GetModelFromWorld ----- + public: virtual std::size_t GetModelCount( + const Identity &_worldID) const override; + + public: virtual Identity GetModel( + const Identity &_worldID, std::size_t _modelIndex) const override; + + public: virtual Identity GetModel( + const Identity &_worldID, const std::string &_modelName) const override; + + public: virtual const std::string &GetModelName( + const Identity &_modelID) const override; + + public: virtual std::size_t GetModelIndex( + const Identity &_modelID) const override; + + public: virtual Identity GetWorldOfModel( + const Identity &_modelID) const override; + + // ----- GetWorldFromEngine ----- + public: virtual std::size_t GetWorldCount( + const Identity &_engineID) const override; + + public: virtual Identity GetWorld( + const Identity &_engineID, std::size_t _worldIndex) const override; + + public: virtual Identity GetWorld( + const Identity &_engineID, const std::string &_worldName) const override; + + public: virtual const std::string &GetWorldName( + const Identity &_worldID) const override; + + public: virtual std::size_t GetWorldIndex( + const Identity &_worldID) const override; + + public: virtual Identity GetEngineOfWorld( + const Identity &_worldID) const override; + // ----- GetLinkFromModel ----- public: std::size_t GetLinkCount( const Identity &_modelID) const override; diff --git a/bullet-featherstone/src/KinematicsFeatures.cc b/bullet-featherstone/src/KinematicsFeatures.cc index 5d77ab3a6..db571902e 100644 --- a/bullet-featherstone/src/KinematicsFeatures.cc +++ b/bullet-featherstone/src/KinematicsFeatures.cc @@ -38,14 +38,7 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( { const auto index = *indexOpt; FrameData data; - data.pose.translation() = convert( - model->body->localPosToWorld(index, btVector3(0, 0, 0))); - data.pose.linear() = convert( - model->body->localFrameToWorld(index, btMatrix3x3::getIdentity())); - - // Transform from bullet's inertia frame to Gazebo's link frame - data.pose = data.pose * linkInfo->inertiaToLinkFrame; - + data.pose = GetWorldTransformOfLink(*model, *linkInfo); const auto &link = model->body->getLink(index); data.linearVelocity = convert(link.m_absFrameTotVelocity.getLinear()); data.angularVelocity = convert(link.m_absFrameTotVelocity.getAngular()); diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 9f992c00f..43d0bd6a4 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -657,9 +657,9 @@ bool SDFFeatures::AddSdfCollision( mu = ode->Mu(); mu2 = ode->Mu2(); } - if (friction->Element()) + if (const auto frictionElement = friction->Element()) { - if (const auto bullet = friction->Element()->GetElement("bullet")) + if (const auto bullet = frictionElement->GetElement("bullet")) { if (const auto f1 = bullet->GetElement("friction")) mu = f1->Get(); @@ -675,9 +675,9 @@ bool SDFFeatures::AddSdfCollision( } } - if (surface->Element()) + if (const auto surfaceElement = surface->Element()) { - if (const auto bounce = surface->Element()->GetElement("bounce")) + if (const auto bounce = surfaceElement->GetElement("bounce")) { if (const auto r = bounce->GetElement("restitution_coefficient")) restitution = r->Get(); diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc index a40c37eb8..0aeb4e042 100644 --- a/bullet-featherstone/src/SimulationFeatures.cc +++ b/bullet-featherstone/src/SimulationFeatures.cc @@ -17,23 +17,69 @@ #include "SimulationFeatures.hh" +#include + +#include +#include + namespace gz { namespace physics { namespace bullet_featherstone { +///////////////////////////////////////////////// void SimulationFeatures::WorldForwardStep( const Identity &_worldID, - ForwardStep::Output & /*_h*/, + ForwardStep::Output & _h, ForwardStep::State & /*_x*/, const ForwardStep::Input & _u) { - const WorldInfoPtr &worldInfo = this->worlds.at(_worldID); - auto *dtDur = - _u.Query(); + const auto worldInfo = this->ReferenceInterface(_worldID); + auto *dtDur = + _u.Query(); + if (dtDur) + { std::chrono::duration dt = *dtDur; - worldInfo->world->stepSimulation(dt.count(), 1, dt.count()); + stepSize = dt.count(); + } + + worldInfo->world->stepSimulation(this->stepSize, 1, this->stepSize); + + this->Write(_h.Get()); } +///////////////////////////////////////////////// +void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const +{ + // remove link poses from the previous iteration + _changedPoses.entries.clear(); + _changedPoses.entries.reserve(this->links.size()); + + std::unordered_map newPoses; + + for (const auto &[id, info] : this->links) + { + const auto &model = this->ReferenceInterface(info->model); + WorldPose wp; + wp.pose = gz::math::eigen3::convert(GetWorldTransformOfLink(*model, *info)); + wp.body = id; + + auto iter = this->prevLinkPoses.find(id); + if ((iter == this->prevLinkPoses.end()) || + !iter->second.Pos().Equal(wp.pose.Pos(), 1e-6) || + !iter->second.Rot().Equal(wp.pose.Rot(), 1e-6)) + { + _changedPoses.entries.push_back(wp); + newPoses[id] = wp.pose; + } + else + newPoses[id] = iter->second; + } + + // Save the new poses so that they can be used to check for updates in the + // next iteration. Re-setting this->prevLinkPoses with the contents of + // newPoses ensures that we aren't caching data for links that were removed + this->prevLinkPoses = std::move(newPoses); +} } // namespace bullet_featherstone } // namespace physics } // namespace gz diff --git a/bullet-featherstone/src/SimulationFeatures.hh b/bullet-featherstone/src/SimulationFeatures.hh index 7be1539cf..32ecfd9fc 100644 --- a/bullet-featherstone/src/SimulationFeatures.hh +++ b/bullet-featherstone/src/SimulationFeatures.hh @@ -18,7 +18,9 @@ #ifndef GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_SIMULATIONFEATURES_HH_ #define GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_SIMULATIONFEATURES_HH_ +#include #include + #include #include "Base.hh" @@ -40,6 +42,14 @@ class SimulationFeatures : ForwardStep::Output &_h, ForwardStep::State &_x, const ForwardStep::Input &_u) override; + + public: void Write(ChangedWorldPoses &_changedPoses) const; + + private: double stepSize = 0.001; + + /// \brief link poses from the most recent pose change/update. + /// The key is the link's ID, and the value is the link's pose + private: mutable std::unordered_map prevLinkPoses; }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/WorldFeatures.cc b/bullet-featherstone/src/WorldFeatures.cc new file mode 100644 index 000000000..30250004f --- /dev/null +++ b/bullet-featherstone/src/WorldFeatures.cc @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2022 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 "WorldFeatures.hh" + +namespace gz { +namespace physics { +namespace bullet_featherstone { + +///////////////////////////////////////////////// +void WorldFeatures::SetWorldGravity( + const Identity &_id, const LinearVectorType &_gravity) +{ + auto worldInfo = this->ReferenceInterface(_id); + if (worldInfo) + worldInfo->world->setGravity( + btVector3(_gravity(0), _gravity(1), _gravity(2))); +} + +///////////////////////////////////////////////// +WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( + const Identity &_id) const +{ + const auto worldInfo = this->ReferenceInterface(_id); + if (worldInfo) + { + // auto world = this->ReferenceInterface(_id); + return WorldFeatures::LinearVectorType( + worldInfo->world->getGravity().x(), + worldInfo->world->getGravity().y(), + worldInfo->world->getGravity().z()); + } + else + { + return WorldFeatures::LinearVectorType(0, 0, 0); + } +} +} +} +} diff --git a/bullet-featherstone/src/WorldFeatures.hh b/bullet-featherstone/src/WorldFeatures.hh new file mode 100644 index 000000000..aefa84fd1 --- /dev/null +++ b/bullet-featherstone/src/WorldFeatures.hh @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2022 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_PHYSICS_BULLET_SRC_WORLDFEATURES_HH_ +#define GZ_PHYSICS_BULLET_SRC_WORLDFEATURES_HH_ + +#include + +#include + +#include "Base.hh" + +namespace gz { +namespace physics { +namespace bullet_featherstone { + +struct WorldFeatureList : FeatureList< + Gravity +> { }; + +class WorldFeatures : + public virtual Base, + public virtual Implements3d +{ + // Documentation inherited + public: void SetWorldGravity( + const Identity &_id, const LinearVectorType &_gravity) override; + + // Documentation inherited + public: LinearVectorType GetWorldGravity(const Identity &_id) const override; +}; + +} +} +} + +#endif diff --git a/bullet-featherstone/src/plugin.cc b/bullet-featherstone/src/plugin.cc index 483b4fe25..d9a1da0f5 100644 --- a/bullet-featherstone/src/plugin.cc +++ b/bullet-featherstone/src/plugin.cc @@ -28,6 +28,7 @@ #include "FreeGroupFeatures.hh" #include "ShapeFeatures.hh" #include "JointFeatures.hh" +#include "WorldFeatures.hh" namespace gz { namespace physics { @@ -40,7 +41,8 @@ struct BulletFeatures : FeatureList < KinematicsFeatureList, SDFFeatureList, ShapeFeatureList, - JointFeatureList + JointFeatureList, + WorldFeatureList > { }; class Plugin : @@ -52,7 +54,8 @@ class Plugin : public virtual KinematicsFeatures, public virtual SDFFeatures, public virtual ShapeFeatures, - public virtual JointFeatures + public virtual JointFeatures, + public virtual WorldFeatures {}; GZ_PHYSICS_ADD_PLUGIN(Plugin, FeaturePolicy3d, BulletFeatures) diff --git a/bullet/CMakeLists.txt b/bullet/CMakeLists.txt index 62219d95f..ccd775167 100644 --- a/bullet/CMakeLists.txt +++ b/bullet/CMakeLists.txt @@ -9,7 +9,7 @@ target_link_libraries(${features} INTERFACE GzBullet::GzBullet) gz_get_libsources_and_unittests(sources test_sources) -# TODO(MXG): Think about an gz_add_plugin(~) macro for ign-cmake +# TODO(MXG): Think about a gz_add_plugin(~) macro for gz-cmake set(engine_name bullet-plugin) gz_add_component(${engine_name} SOURCES ${sources} @@ -30,7 +30,7 @@ install(TARGETS ${bullet_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR}) # Install redirection headers install( DIRECTORY include/ - DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}") + DESTINATION "${GZ_INCLUDE_INSTALL_DIR_FULL}") # The library created by `gz_add_component` includes the gz-physics version # (i.e. libgz-physics1-name-plugin.so), but for portability, diff --git a/bullet/src/SimulationFeatures.cc b/bullet/src/SimulationFeatures.cc index 6bb134243..4ed62aa0f 100644 --- a/bullet/src/SimulationFeatures.cc +++ b/bullet/src/SimulationFeatures.cc @@ -17,22 +17,69 @@ #include "SimulationFeatures.hh" +#include +#include + namespace gz { namespace physics { namespace bullet { +///////////////////////////////////////////////// void SimulationFeatures::WorldForwardStep( const Identity &_worldID, - ForwardStep::Output & /*_h*/, + ForwardStep::Output & _h, ForwardStep::State & /*_x*/, const ForwardStep::Input & _u) { - const WorldInfoPtr &worldInfo = this->worlds.at(_worldID); + const WorldInfoPtr &worldInfo = this->worlds.at(_worldID); - auto *dtDur = - _u.Query(); + auto *dtDur = + _u.Query(); + if (dtDur) + { std::chrono::duration dt = *dtDur; - worldInfo->world->stepSimulation(dt.count(), 1, dt.count()); + stepSize = dt.count(); + } + + worldInfo->world->stepSimulation(this->stepSize, 1, this->stepSize); + this->Write(_h.Get()); +} + +///////////////////////////////////////////////// +void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const +{ + // remove link poses from the previous iteration + _changedPoses.entries.clear(); + _changedPoses.entries.reserve(this->links.size()); + + std::unordered_map newPoses; + + for (const auto &[id, info] : this->links) + { + // make sure the link exists + if (info) + { + WorldPose wp; + wp.pose = info->pose; + wp.body = id; + + auto iter = this->prevLinkPoses.find(id); + if ((iter == this->prevLinkPoses.end()) || + !iter->second.Pos().Equal(wp.pose.Pos(), 1e-6) || + !iter->second.Rot().Equal(wp.pose.Rot(), 1e-6)) + { + _changedPoses.entries.push_back(wp); + newPoses[id] = wp.pose; + } + else + newPoses[id] = iter->second; + } + } + + // Save the new poses so that they can be used to check for updates in the + // next iteration. Re-setting this->prevLinkPoses with the contents of + // newPoses ensures that we aren't caching data for links that were removed + this->prevLinkPoses = std::move(newPoses); } } // namespace bullet diff --git a/bullet/src/SimulationFeatures.hh b/bullet/src/SimulationFeatures.hh index 286b5da7b..9df3337a4 100644 --- a/bullet/src/SimulationFeatures.hh +++ b/bullet/src/SimulationFeatures.hh @@ -18,7 +18,9 @@ #ifndef GZ_PHYSICS_BULLET_SRC_SIMULATIONFEATURES_HH_ #define GZ_PHYSICS_BULLET_SRC_SIMULATIONFEATURES_HH_ +#include #include + #include #include "Base.hh" @@ -40,6 +42,14 @@ class SimulationFeatures : ForwardStep::Output &_h, ForwardStep::State &_x, const ForwardStep::Input &_u) override; + + public: void Write(ChangedWorldPoses &_changedPoses) const; + + private: double stepSize = 0.001; + + /// \brief link poses from the most recent pose change/update. + /// The key is the link's ID, and the value is the link's pose + private: mutable std::unordered_map prevLinkPoses; }; } // namespace bullet diff --git a/configure.bat b/configure.bat deleted file mode 100644 index 71b94d7cb..000000000 --- a/configure.bat +++ /dev/null @@ -1,26 +0,0 @@ - -:: NOTE: This script is only meant to be used as part of the ignition developers' CI system -:: Users and developers should build and install this library using cmake and Visual Studio - - -:: Install dependencies -::call %win_lib% :download_unzip_install bullet dart -call %win_lib% :download_unzip_install eigen3-3.3.4.zip -call %win_lib% :install_ign_project ign-plugin default - -:: Set configuration variables -@set build_type=Release -@if not "%1"=="" set build_type=%1 -@echo Configuring for build type %build_type% - -:: Go to the directory that this configure.bat file exists in -cd /d %~dp0 - -:: Create a build directory and configure -md build -cd build -cmake .. -G "NMake Makefiles" -DCMAKE_INSTALL_PREFIX="%WORKSPACE_INSTALL_DIR%" -DCMAKE_BUILD_TYPE="%build_type%" -DBUILD_TESTING:BOOL=False -:: Note: We disable testing by default. If the intention is for the CI to build and test -:: this project, then the CI script will turn it back on. - -:: If the caller wants to build and/or install, they should do so after calling this script diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index cbaeb245b..c8756ab83 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -13,11 +13,11 @@ endif() install( DIRECTORY include/ - DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}") + DESTINATION "${GZ_INCLUDE_INSTALL_DIR_FULL}") gz_get_libsources_and_unittests(sources test_sources) -# TODO(MXG): Think about an gz_add_plugin(~) macro for ign-cmake +# TODO(MXG): Think about a gz_add_plugin(~) macro for gz-cmake set(engine_name dartsim-plugin) gz_add_component(${engine_name} SOURCES ${sources} diff --git a/dartsim/src/CustomHeightmapShape.cc b/dartsim/src/CustomHeightmapShape.cc index 630221aaf..0ae3e370a 100644 --- a/dartsim/src/CustomHeightmapShape.cc +++ b/dartsim/src/CustomHeightmapShape.cc @@ -47,10 +47,10 @@ CustomHeightmapShape::CustomHeightmapShape( else scale.Z(fabs(_size(2)) / heightmapSizeZ); - auto sizeIgn = gz::math::eigen3::convert(_size); + auto sizeGz = gz::math::eigen3::convert(_size); std::vector heightsFloat; - _input.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY, + _input.FillHeightMap(_subSampling, vertSize, sizeGz, scale, flipY, heightsFloat); this->setHeightField(vertSize, vertSize, heightsFloat); diff --git a/dartsim/src/CustomHeightmapShape.hh b/dartsim/src/CustomHeightmapShape.hh index dfd83e394..3358df8af 100644 --- a/dartsim/src/CustomHeightmapShape.hh +++ b/dartsim/src/CustomHeightmapShape.hh @@ -26,7 +26,7 @@ namespace physics { namespace dartsim { /// \brief This class creates a custom derivative of dartsim's HeightmapShape -/// class which allows an gz::common::Heightmap to be converted into a +/// class which allows a gz::common::Heightmap to be converted into a /// HeightmapShape that can be used by dartsim. /// Using float precision because Bullet's collision detector doesn't support /// double. common::HeightmapData also holds floats. diff --git a/dartsim/src/CustomMeshShape.hh b/dartsim/src/CustomMeshShape.hh index 1f7531b55..4a30b55f9 100644 --- a/dartsim/src/CustomMeshShape.hh +++ b/dartsim/src/CustomMeshShape.hh @@ -26,7 +26,7 @@ namespace physics { namespace dartsim { /// \brief This class creates a custom derivative of dartsim's MeshShape class -/// which allows an gz::common::Mesh to be converted into a MeshShape that +/// which allows a gz::common::Mesh to be converted into a MeshShape that /// can be used by dartsim. class CustomMeshShape : public dart::dynamics::MeshShape { diff --git a/dartsim/src/FreeGroupFeatures_TEST.cc b/dartsim/src/FreeGroupFeatures_TEST.cc deleted file mode 100644 index f2a17160d..000000000 --- a/dartsim/src/FreeGroupFeatures_TEST.cc +++ /dev/null @@ -1,194 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include "gz/physics/FrameSemantics.hh" -#include "gz/physics/FreeGroup.hh" -#include "gz/physics/GetEntities.hh" -#include "gz/physics/RequestEngine.hh" -#include "gz/physics/sdf/ConstructModel.hh" -#include "gz/physics/sdf/ConstructNestedModel.hh" -#include "gz/physics/sdf/ConstructWorld.hh" -#include "test/Utils.hh" - -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::sdf::ConstructSdfWorld, - gz::physics::sdf::ConstructSdfModel, - gz::physics::sdf::ConstructSdfNestedModel, - gz::physics::GetWorldFromEngine, - gz::physics::GetModelFromWorld, - gz::physics::GetNestedModelFromModel, - gz::physics::GetLinkFromModel, - gz::physics::LinkFrameSemantics, - gz::physics::FindFreeGroupFeature, - gz::physics::SetFreeGroupWorldPose > { }; - -using World = gz::physics::World3d; -using WorldPtr = gz::physics::World3dPtr; -using ModelPtr = gz::physics::Model3dPtr; -using LinkPtr = gz::physics::Link3dPtr; - -///////////////////////////////////////////////// -auto LoadEngine() -{ - gz::plugin::Loader loader; - loader.LoadLib(dartsim_plugin_LIB); - - gz::plugin::PluginPtr dartsim = - loader.Instantiate("gz::physics::dartsim::Plugin"); - - auto engine = - gz::physics::RequestEngine3d::From(dartsim); - return engine; -} - -///////////////////////////////////////////////// -WorldPtr LoadWorld(const std::string &_world) -{ - auto engine = LoadEngine(); - EXPECT_NE(nullptr, engine); - if (nullptr == engine) - { - return nullptr; - } - - sdf::Root root; - const sdf::Errors &errors = root.Load(_world); - EXPECT_EQ(0u, errors.size()) << errors; - - EXPECT_EQ(1u, root.WorldCount()); - const sdf::World *sdfWorld = root.WorldByIndex(0); - EXPECT_NE(nullptr, sdfWorld); - - auto world = engine->ConstructWorld(*sdfWorld); - EXPECT_NE(nullptr, world); - - return world; -} - -ModelPtr GetModelFromAbsoluteName(const WorldPtr &_world, - const std::string &_absoluteName) -{ - std::vector names = - gz::common::split(_absoluteName, sdf::kSdfScopeDelimiter); - if (names.empty()) - { - return nullptr; - } - - auto currentModel = _world->GetModel(names.front()); - for (std::size_t i = 1; i < names.size(); ++i) - { - if (nullptr == currentModel) - return nullptr; - currentModel = currentModel->GetNestedModel(names[i]); - } - return currentModel; -} - -TEST(FreeGroupFeatures, NestedFreeGroup) -{ - WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); - ASSERT_NE(nullptr, world); - - auto checkFreeGroupForModel = [&](const std::string &_modelName) - { - auto model = GetModelFromAbsoluteName(world, _modelName); - if (nullptr == model) - return testing::AssertionFailure() << "Model could not be found"; - auto freeGroup = model->FindFreeGroup(); - if (nullptr == freeGroup) - return testing::AssertionFailure() << "Freegroup could not be found"; - if (nullptr == freeGroup->RootLink()) - return testing::AssertionFailure() << "RootLink could not be found"; - return testing::AssertionSuccess(); - }; - - EXPECT_TRUE(checkFreeGroupForModel("parent_model")); - // Expect false because the link in nested_model is referenced by a joint. - EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model")); - EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model2")); - EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model3")); -} - -TEST(FreeGroupFeatures, NestedFreeGroupSetWorldPose) -{ - WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); - ASSERT_NE(nullptr, world); - - gz::math::Pose3d parentModelNewPose(0, 0, 2, 0, 0, 0); - { - auto parentModel = world->GetModel("parent_model"); - ASSERT_NE(nullptr, parentModel); - auto freeGroup = parentModel->FindFreeGroup(); - ASSERT_NE(nullptr, freeGroup); - - freeGroup->SetWorldPose( - gz::math::eigen3::convert(parentModelNewPose)); - - auto link1 = parentModel->GetLink("link1"); - ASSERT_NE(nullptr, link1); - auto frameData = link1->FrameDataRelativeToWorld(); - EXPECT_EQ(parentModelNewPose, - gz::math::eigen3::convert(frameData.pose)); - } - { - auto nestedModel = - GetModelFromAbsoluteName(world, "parent_model::nested_model"); - ASSERT_NE(nullptr, nestedModel); - auto nestedLink1 = nestedModel->GetLink("nested_link1"); - ASSERT_NE(nullptr, nestedLink1); - auto frameData = nestedLink1->FrameDataRelativeToWorld(); - // Poses from SDF - gz::math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0); - gz::math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, GZ_PI_2); - - gz::math::Pose3d nestedLinkExpectedPose = - parentModelNewPose * nestedModelPose * nestedLinkPose; - - EXPECT_EQ(nestedLinkExpectedPose, - gz::math::eigen3::convert(frameData.pose)); - } - { - auto parentModel = world->GetModel("parent_model2"); - ASSERT_NE(nullptr, parentModel); - auto freeGroup = parentModel->FindFreeGroup(); - ASSERT_NE(nullptr, freeGroup); - - freeGroup->SetWorldPose( - gz::math::eigen3::convert(parentModelNewPose)); - - auto grandChild = GetModelFromAbsoluteName( - world, "parent_model2::child_model::grand_child_model"); - ASSERT_NE(nullptr, grandChild); - auto link1 = grandChild->GetLink("link1"); - ASSERT_NE(nullptr, link1); - auto frameData = link1->FrameDataRelativeToWorld(); - EXPECT_EQ(parentModelNewPose, - gz::math::eigen3::convert(frameData.pose)); - } -} diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 8421a7ba6..d504d2982 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -834,7 +834,7 @@ TEST_F(JointFeaturesFixture, JointAttachDetach) // the same as it was before they were attached fixedJoint->SetTransformFromParent(poseParentChild); - // The name of the link obtained using the ign-physics API should remain the + // The name of the link obtained using the gz-physics API should remain the // same even though AttachFixedJoint renames the associated BodyNode. EXPECT_EQ(bodyName, model2Body->GetName()); @@ -855,7 +855,7 @@ TEST_F(JointFeaturesFixture, JointAttachDetach) // now detach joint and expect model2 to start moving again fixedJoint->Detach(); - // The name of the link obtained using the ign-physics API should remain the + // The name of the link obtained using the gz-physics API should remain the // same even though Detach renames the associated BodyNode. EXPECT_EQ(bodyName, model2Body->GetName()); @@ -1030,7 +1030,7 @@ TEST_F(JointFeaturesFixture, JointAttachDetachSpawnedModel) const auto poseParent = dartBody1->getTransform(); const auto poseChild = dartBody2->getTransform(); - // Before ign-physics PR #31, uncommenting the following `step` call makes + // Before gz-physics PR #31, uncommenting the following `step` call makes // this test pass, but commenting it out makes it fail. // world->Step(output, state, input); auto fixedJoint = model2Body->AttachFixedJoint(model1Body); diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 628489f70..7a41f9a5b 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -665,7 +665,7 @@ Identity SDFFeatures::ConstructSdfLink( this->ConstructSdfCollision(linkIdentity, *collision); } - // ign-physics is currently ignoring visuals, so we won't parse them from the + // gz-physics is currently ignoring visuals, so we won't parse them from the // SDF // for (std::size_t i = 0; i < _sdfLink.VisualCount(); ++i) // { diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 8bf336d8a..ebe0360c7 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -172,7 +172,7 @@ void SimulationFeatures::AddContactPropertiesCallback( { auto *world = this->ReferenceInterface(_worldID); - auto handler = std::make_shared(); + auto handler = std::make_shared(); handler->surfaceParamsCallback = _callback; handler->convertContact = [this](const dart::collision::Contact& _contact) { return this->convertContact(_contact); @@ -202,7 +202,7 @@ bool SimulationFeatures::RemoveContactPropertiesCallback( } } -dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( +dart::constraint::ContactSurfaceParams GzContactSurfaceHandler::createParams( const dart::collision::Contact& _contact, const size_t _numContactsOnCollisionObject) const { @@ -214,40 +214,40 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( typedef SetContactPropertiesCallbackFeature F; typedef FeaturePolicy3d P; - typename F::ContactSurfaceParams

pIgn; + typename F::ContactSurfaceParams

pGz; - pIgn.frictionCoeff = pDart.mFrictionCoeff; - pIgn.secondaryFrictionCoeff = pDart.mSecondaryFrictionCoeff; - pIgn.slipCompliance = pDart.mSlipCompliance; - pIgn.secondarySlipCompliance = pDart.mSecondarySlipCompliance; - pIgn.restitutionCoeff = pDart.mRestitutionCoeff; - pIgn.firstFrictionalDirection = pDart.mFirstFrictionalDirection; - pIgn.contactSurfaceMotionVelocity = pDart.mContactSurfaceMotionVelocity; + pGz.frictionCoeff = pDart.mFrictionCoeff; + pGz.secondaryFrictionCoeff = pDart.mSecondaryFrictionCoeff; + pGz.slipCompliance = pDart.mSlipCompliance; + pGz.secondarySlipCompliance = pDart.mSecondarySlipCompliance; + pGz.restitutionCoeff = pDart.mRestitutionCoeff; + pGz.firstFrictionalDirection = pDart.mFirstFrictionalDirection; + pGz.contactSurfaceMotionVelocity = pDart.mContactSurfaceMotionVelocity; auto contactInternal = this->convertContact(_contact); if (contactInternal) { this->surfaceParamsCallback(contactInternal.value(), - _numContactsOnCollisionObject, pIgn); - - if (pIgn.frictionCoeff) - pDart.mFrictionCoeff = pIgn.frictionCoeff.value(); - if (pIgn.secondaryFrictionCoeff) - pDart.mSecondaryFrictionCoeff = pIgn.secondaryFrictionCoeff.value(); - if (pIgn.slipCompliance) - pDart.mSlipCompliance = pIgn.slipCompliance.value(); - if (pIgn.secondarySlipCompliance) - pDart.mSecondarySlipCompliance = pIgn.secondarySlipCompliance.value(); - if (pIgn.restitutionCoeff) - pDart.mRestitutionCoeff = pIgn.restitutionCoeff.value(); - if (pIgn.firstFrictionalDirection) - pDart.mFirstFrictionalDirection = pIgn.firstFrictionalDirection.value(); - if (pIgn.contactSurfaceMotionVelocity) + _numContactsOnCollisionObject, pGz); + + if (pGz.frictionCoeff) + pDart.mFrictionCoeff = pGz.frictionCoeff.value(); + if (pGz.secondaryFrictionCoeff) + pDart.mSecondaryFrictionCoeff = pGz.secondaryFrictionCoeff.value(); + if (pGz.slipCompliance) + pDart.mSlipCompliance = pGz.slipCompliance.value(); + if (pGz.secondarySlipCompliance) + pDart.mSecondarySlipCompliance = pGz.secondarySlipCompliance.value(); + if (pGz.restitutionCoeff) + pDart.mRestitutionCoeff = pGz.restitutionCoeff.value(); + if (pGz.firstFrictionalDirection) + pDart.mFirstFrictionalDirection = pGz.firstFrictionalDirection.value(); + if (pGz.contactSurfaceMotionVelocity) pDart.mContactSurfaceMotionVelocity = - pIgn.contactSurfaceMotionVelocity.value(); + pGz.contactSurfaceMotionVelocity.value(); static bool warnedRollingFrictionCoeff = false; - if (!warnedRollingFrictionCoeff && pIgn.rollingFrictionCoeff) + if (!warnedRollingFrictionCoeff && pGz.rollingFrictionCoeff) { gzwarn << "DART doesn't support rolling friction setting" << std::endl; warnedRollingFrictionCoeff = true; @@ -255,7 +255,7 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( static bool warnedSecondaryRollingFrictionCoeff = false; if (!warnedSecondaryRollingFrictionCoeff && - pIgn.secondaryRollingFrictionCoeff) + pGz.secondaryRollingFrictionCoeff) { gzwarn << "DART doesn't support secondary rolling friction setting" << std::endl; @@ -263,7 +263,7 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( } static bool warnedTorsionalFrictionCoeff = false; - if (!warnedTorsionalFrictionCoeff && pIgn.torsionalFrictionCoeff) + if (!warnedTorsionalFrictionCoeff && pGz.torsionalFrictionCoeff) { gzwarn << "DART doesn't support torsional friction setting" << std::endl; @@ -271,36 +271,36 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( } } - this->lastIgnParams = pIgn; + this->lastGzParams = pGz; return pDart; } dart::constraint::ContactConstraintPtr -IgnContactSurfaceHandler::createConstraint( +GzContactSurfaceHandler::createConstraint( dart::collision::Contact& _contact, const size_t _numContactsOnCollisionObject, const double _timeStep) const { - // this call sets this->lastIgnParams + // this call sets this->lastGzParams auto constraint = dart::constraint::ContactSurfaceHandler::createConstraint( _contact, _numContactsOnCollisionObject, _timeStep); typedef SetContactPropertiesCallbackFeature F; typedef FeaturePolicy3d P; - typename F::ContactSurfaceParams

& p = this->lastIgnParams; + typename F::ContactSurfaceParams

& p = this->lastGzParams; - if (this->lastIgnParams.errorReductionParameter) + if (this->lastGzParams.errorReductionParameter) constraint->setErrorReductionParameter(p.errorReductionParameter.value()); - if (this->lastIgnParams.maxErrorAllowance) + if (this->lastGzParams.maxErrorAllowance) constraint->setErrorAllowance(p.maxErrorAllowance.value()); - if (this->lastIgnParams.maxErrorReductionVelocity) + if (this->lastGzParams.maxErrorReductionVelocity) constraint->setMaxErrorReductionVelocity( p.maxErrorReductionVelocity.value()); - if (this->lastIgnParams.constraintForceMixing) + if (this->lastGzParams.constraintForceMixing) constraint->setConstraintForceMixing(p.constraintForceMixing.value()); return constraint; diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index 3a27f90cf..3806db25d 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -60,7 +60,7 @@ struct SimulationFeatureList : FeatureList< > { }; #ifdef DART_HAS_CONTACT_SURFACE -class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler +class GzContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler { public: dart::constraint::ContactSurfaceParams createParams( const dart::collision::Contact& _contact, @@ -81,10 +81,10 @@ class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler > convertContact; public: mutable typename Feature::ContactSurfaceParams - lastIgnParams; + lastGzParams; }; -using IgnContactSurfaceHandlerPtr = std::shared_ptr; +using GzContactSurfaceHandlerPtr = std::shared_ptr; #endif class SimulationFeatures : @@ -127,7 +127,7 @@ class SimulationFeatures : const Identity &_worldID, const std::string &_callbackID) override; private: std::unordered_map< - std::string, IgnContactSurfaceHandlerPtr> contactSurfaceHandlers; + std::string, GzContactSurfaceHandlerPtr> contactSurfaceHandlers; #endif }; diff --git a/heightmap/CMakeLists.txt b/heightmap/CMakeLists.txt index 2012b5b98..4dd857c11 100644 --- a/heightmap/CMakeLists.txt +++ b/heightmap/CMakeLists.txt @@ -7,4 +7,4 @@ target_link_libraries(${heightmap} install( DIRECTORY include/ - DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}") + DESTINATION "${GZ_INCLUDE_INSTALL_DIR_FULL}") diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 4b2bdd7bb..992a1312e 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -1,2 +1,2 @@ add_subdirectory(gz) -install(DIRECTORY ignition DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}) +install(DIRECTORY ignition DESTINATION ${GZ_INCLUDE_INSTALL_DIR_FULL}) diff --git a/include/gz/physics/ForwardStep.hh b/include/gz/physics/ForwardStep.hh index 01e00fc03..7b282d7aa 100644 --- a/include/gz/physics/ForwardStep.hh +++ b/include/gz/physics/ForwardStep.hh @@ -155,7 +155,8 @@ namespace gz ApplyExternalForceTorques, ApplyGeneralizedForces, VelocityControlCommands, - ServoControlCommands>; + ServoControlCommands, + std::chrono::steady_clock::duration>; public: using Output = SpecifyData< RequireData, diff --git a/include/gz/physics/TemplateHelpers.hh b/include/gz/physics/TemplateHelpers.hh index 08a6242ba..e01a16f83 100644 --- a/include/gz/physics/TemplateHelpers.hh +++ b/include/gz/physics/TemplateHelpers.hh @@ -66,7 +66,7 @@ namespace gz /// \brief Use this macro to create an API "selector" for a custom class. /// /// Features may define APIs for class types that are not anticipated ahead of -/// time by the ign-physics library. When aggregating the API for that class +/// time by the gz-physics library. When aggregating the API for that class /// from a set of features, the Aggregator must be given a Selector that can /// ignore features that don't mention the class (or else a compilation failure /// would occur). diff --git a/include/gz/physics/detail/RelativeQuantity.hh b/include/gz/physics/detail/RelativeQuantity.hh index 1d54070d1..6edfaf7a7 100644 --- a/include/gz/physics/detail/RelativeQuantity.hh +++ b/include/gz/physics/detail/RelativeQuantity.hh @@ -477,8 +477,8 @@ namespace gz const Quantity &_box, const RotationType &/*_currentCoordinates*/) { - // TODO(anyone): Replace with gzwarn when/if we add an ign console - // dependency to ign-physics + // TODO(anyone): Replace with gzwarn when/if we add a gz console + // dependency to gz-physics std::cerr << "[AABBSpace::ResolveToWorldCoordinates] Warning: " << "Axis-aligned bounding boxes cannot undergo coordinate " << "changes.\n"; @@ -490,8 +490,8 @@ namespace gz const RotationType &/*_currentCoordinates*/, const RotationType &/*_targetCoordinates*/) { - // TODO(anyone): Replace with gzwarn when/if we add an ign console - // dependency to ign-physics + // TODO(anyone): Replace with gzwarn when/if we add a gz console + // dependency to gz-physics std::cerr << "[AABBSpace::ResolveToTargetCoordinates] Warning: " << "Axis-aligned bounding boxes cannot undergo coordinate " << "changes.\n"; diff --git a/mesh/CMakeLists.txt b/mesh/CMakeLists.txt index af5885524..96af77285 100644 --- a/mesh/CMakeLists.txt +++ b/mesh/CMakeLists.txt @@ -7,4 +7,4 @@ target_link_libraries(${mesh} install( DIRECTORY include/ - DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}") + DESTINATION "${GZ_INCLUDE_INSTALL_DIR_FULL}") diff --git a/sdf/CMakeLists.txt b/sdf/CMakeLists.txt index b2d661306..0917bdf91 100644 --- a/sdf/CMakeLists.txt +++ b/sdf/CMakeLists.txt @@ -5,4 +5,4 @@ target_link_libraries(${sdf} INTERFACE ${SDFormat_LIBRARIES}) install( DIRECTORY include/ - DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}") + DESTINATION "${GZ_INCLUDE_INSTALL_DIR_FULL}") diff --git a/sdf/include/gz/physics/sdf/ConstructVisual.hh b/sdf/include/gz/physics/sdf/ConstructVisual.hh index 6400fa2eb..da82a549b 100644 --- a/sdf/include/gz/physics/sdf/ConstructVisual.hh +++ b/sdf/include/gz/physics/sdf/ConstructVisual.hh @@ -32,7 +32,7 @@ class ConstructSdfVisual : public virtual Feature class Link : public virtual Feature::Link { // TODO(MXG): Return a Shape type instead of a bool once we have shape - // features in the core ign-physics library. + // features in the core gz-physics library. public: bool ConstructVisual(const ::sdf::Visual &_visual); }; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 84f4f9011..14f4f830f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,5 +1,5 @@ gz_get_libsources_and_unittests(sources gtest_sources) -gz_create_core_library(SOURCES ${sources}) +gz_create_core_library(SOURCES ${sources} CXX_STANDARD 17) target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index d1b00f4a0..ed0e79427 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,5 +1,5 @@ #============================================================================ -# Do a fake install of ign-physics in order to test the examples. +# Do a fake install of gz-physics in order to test the examples. #============================================================================ set(FAKE_INSTALL_PREFIX "${CMAKE_BINARY_DIR}/fake/install") diff --git a/test/MockCenterOfMass.hh b/test/MockCenterOfMass.hh index 4adad8a31..d2da10bd7 100644 --- a/test/MockCenterOfMass.hh +++ b/test/MockCenterOfMass.hh @@ -24,7 +24,7 @@ namespace mock { ///////////////////////////////////////////////// - // TODO(MXG): Offer an gz::physics::Vector class that accepts a + // TODO(MXG): Offer a gz::physics::Vector class that accepts a // FeaturePolicy type. template using Vector = Eigen::Matrix< diff --git a/test/benchmark/CMakeLists.txt b/test/benchmark/CMakeLists.txt index 0dc1c49bc..292a4f003 100644 --- a/test/benchmark/CMakeLists.txt +++ b/test/benchmark/CMakeLists.txt @@ -1,4 +1,4 @@ -include(IgnBenchmark) +include(GzBenchmark) set(tests ExpectData.cc diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 383fd21f5..7c1324836 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -3,7 +3,9 @@ set(TEST_TYPE "COMMON_TEST") set(tests basic_test construct_empty_world + free_joint_features simulation_features + world_features ) function(configure_common_test PHYSICS_ENGINE_NAME test_name) diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc new file mode 100644 index 000000000..accc0f64f --- /dev/null +++ b/test/common_test/free_joint_features.cc @@ -0,0 +1,235 @@ +/* + * Copyright (C) 2022 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 "../helpers/TestLibLoader.hh" + +#include +#include +#include +#include +#include +#include +#include + +#include "gz/physics/FindFeatures.hh" +#include "gz/physics/FrameSemantics.hh" +#include "gz/physics/FreeGroup.hh" +#include "gz/physics/GetEntities.hh" +#include "gz/physics/RequestEngine.hh" +#include "gz/physics/sdf/ConstructModel.hh" +#include "gz/physics/sdf/ConstructNestedModel.hh" +#include "gz/physics/sdf/ConstructWorld.hh" +// #include "test/Utils.hh" + +struct TestFeatureList : gz::physics::FeatureList< + gz::physics::GetEngineInfo, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfNestedModel, + gz::physics::GetWorldFromEngine, + gz::physics::GetModelFromWorld, + gz::physics::GetNestedModelFromModel, + gz::physics::GetLinkFromModel, + gz::physics::LinkFrameSemantics, + gz::physics::FindFreeGroupFeature, + gz::physics::SetFreeGroupWorldPose > { }; + +using World = gz::physics::World3d; +using WorldPtr = gz::physics::World3dPtr; +using ModelPtr = gz::physics::Model3dPtr; +using LinkPtr = gz::physics::Link3dPtr; + +class FreeGroupFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + loader.LoadLib(FreeGroupFeaturesTest::GetLibToTest()); + + // TODO(ahcorde): We should also run the 3f, 2d, and 2f variants of + // FindFeatures + pluginNames = gz::physics::FindFeatures3d::From(loader); + if (pluginNames.empty()) + { + std::cerr << "No plugins with required features found in " + << GetLibToTest() << std::endl; + GTEST_SKIP(); + } + } + + public: std::set pluginNames; + public: gz::plugin::Loader loader; +}; + +ModelPtr GetModelFromAbsoluteName(const WorldPtr &_world, + const std::string &_absoluteName) +{ + std::vector names = + gz::common::split(_absoluteName, sdf::kSdfScopeDelimiter); + if (names.empty()) + { + return nullptr; + } + + auto currentModel = _world->GetModel(names.front()); + for (std::size_t i = 1; i < names.size(); ++i) + { + if (nullptr == currentModel) + return nullptr; + currentModel = currentModel->GetNestedModel(names[i]); + } + return currentModel; +} + +TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) +{ + for (const std::string &name : pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors &errors = root.Load(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + EXPECT_EQ(0u, errors.size()) << errors; + + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + auto world = engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); + + auto checkFreeGroupForModel = [&](const std::string &_modelName) + { + auto model = GetModelFromAbsoluteName(world, _modelName); + if (nullptr == model) + return testing::AssertionFailure() << "Model could not be found"; + auto freeGroup = model->FindFreeGroup(); + if (nullptr == freeGroup) + return testing::AssertionFailure() << "Freegroup could not be found"; + if (nullptr == freeGroup->RootLink()) + return testing::AssertionFailure() << "RootLink could not be found"; + return testing::AssertionSuccess(); + }; + + EXPECT_TRUE(checkFreeGroupForModel("parent_model")); + if (engine->GetName() == "tpe") + { + // Expect true because tpe doesn't support joints, the link in + // nested_model could not be referenced by a joint. + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model")); + } + else + { + // Expect false because the link in nested_model is referenced by a joint. + EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model")); + } + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model2")); + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model3")); + } +} + +TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) +{ + for (const std::string &name : pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors &errors = root.Load(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + EXPECT_EQ(0u, errors.size()) << errors; + + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + auto world = engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); + + gz::math::Pose3d parentModelNewPose(0, 0, 2, 0, 0, 0); + { + auto parentModel = world->GetModel("parent_model"); + ASSERT_NE(nullptr, parentModel); + auto freeGroup = parentModel->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroup); + + freeGroup->SetWorldPose( + gz::math::eigen3::convert(parentModelNewPose)); + + auto link1 = parentModel->GetLink("link1"); + ASSERT_NE(nullptr, link1); + auto frameData = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(parentModelNewPose, + gz::math::eigen3::convert(frameData.pose)); + } + { + auto nestedModel = + GetModelFromAbsoluteName(world, "parent_model::nested_model"); + ASSERT_NE(nullptr, nestedModel); + auto nestedLink1 = nestedModel->GetLink("nested_link1"); + ASSERT_NE(nullptr, nestedLink1); + auto frameData = nestedLink1->FrameDataRelativeToWorld(); + // Poses from SDF + gz::math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0); + gz::math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, GZ_PI_2); + + gz::math::Pose3d nestedLinkExpectedPose = + parentModelNewPose * nestedModelPose * nestedLinkPose; + + EXPECT_EQ(nestedLinkExpectedPose, + gz::math::eigen3::convert(frameData.pose)); + } + { + auto parentModel = world->GetModel("parent_model2"); + ASSERT_NE(nullptr, parentModel); + auto freeGroup = parentModel->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroup); + + freeGroup->SetWorldPose( + gz::math::eigen3::convert(parentModelNewPose)); + + auto grandChild = GetModelFromAbsoluteName( + world, "parent_model2::child_model::grand_child_model"); + ASSERT_NE(nullptr, grandChild); + auto link1 = grandChild->GetLink("link1"); + ASSERT_NE(nullptr, link1); + auto frameData = link1->FrameDataRelativeToWorld(); + EXPECT_EQ(parentModelNewPose, + gz::math::eigen3::convert(frameData.pose)); + } + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + FreeGroupFeaturesTest::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index d32ed747b..2540820f3 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -21,13 +21,10 @@ #include #include -#include #include #include -#include - #include "../helpers/TestLibLoader.hh" #include @@ -89,7 +86,6 @@ using Features = gz::physics::FeatureList< gz::physics::GetEllipsoidShapeProperties >; -using TestWorldPtr = gz::physics::World3dPtr; using TestContactPoint = gz::physics::World3d::ContactPoint; class SimulationFeaturesTest: @@ -112,6 +108,7 @@ class SimulationFeaturesTest: GTEST_SKIP(); } } + public: std::set pluginNames; public: gz::plugin::Loader loader; }; @@ -138,6 +135,7 @@ std::unordered_set> LoadWorlds( EXPECT_EQ(0u, errors.size()); const sdf::World *sdfWorld = root.WorldByIndex(0); auto world = engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); worlds.insert(world); } @@ -150,9 +148,11 @@ std::unordered_set> LoadWorlds( /// being stepped in (true) or not (false) /// \param[in] _numSteps The number of steps to take in _world /// \return true if the forward step output was checked, false otherwise -bool StepWorld(const TestWorldPtr &_world, bool _firstTime, +template +bool StepWorld(const gz::physics::World3dPtr &_world, bool _firstTime, const std::size_t _numSteps = 1) { + EXPECT_NE(nullptr, _world); gz::physics::ForwardStep::Input input; gz::physics::ForwardStep::State state; gz::physics::ForwardStep::Output output; @@ -177,16 +177,56 @@ bool StepWorld(const TestWorldPtr &_world, bool _firstTime, return checkedOutput; } +template +class StepWorldTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + loader.LoadLib(StepWorldTest::GetLibToTest()); + + // TODO(ahcorde): We should also run the 3f, 2d, and 2f variants of + // FindFeatures + pluginNames = gz::physics::FindFeatures3d::From(loader); + if (pluginNames.empty()) + { + std::cerr << "No plugins with required features found in " + << GetLibToTest() << std::endl; + GTEST_SKIP(); + } + } + + public: std::set pluginNames; + public: gz::plugin::Loader loader; +}; + +// The features that an engine must have to be loaded by this loader. +using StepForwardFeatures = gz::physics::FeatureList< + gz::physics::ConstructEmptyWorldFeature, + gz::physics::ForwardStep, + gz::physics::sdf::ConstructSdfWorld +>; + +template +class StepForwardTestClass : + public StepWorldTest{}; +using StepForwardTestClassTypes = + ::testing::Types; +TYPED_TEST_SUITE(StepForwardTestClass, StepForwardTestClassTypes); + ///////////////////////////////////////////////// -TEST_F(SimulationFeaturesTest, StepWorld) +TYPED_TEST(StepForwardTestClass, StepWorld) { - auto worlds = LoadWorlds( - loader, - pluginNames, + auto worlds = LoadWorlds( + this->loader, + this->pluginNames, gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world")); for (const auto &world : worlds) { - auto checkedOutput = StepWorld(world, true, 1000); + auto checkedOutput = StepWorld(world, true, 1000); EXPECT_TRUE(checkedOutput); } } @@ -225,14 +265,14 @@ using ShapeFeaturesFeatures = gz::physics::FeatureList< gz::physics::GetModelBoundingBox, gz::physics::AttachBoxShapeFeature, - gz::physics::AttachCapsuleShapeFeature, - gz::physics::AttachCylinderShapeFeature, gz::physics::AttachSphereShapeFeature, + gz::physics::AttachCylinderShapeFeature, gz::physics::AttachEllipsoidShapeFeature, + gz::physics::AttachCapsuleShapeFeature, + gz::physics::GetSphereShapeProperties, gz::physics::GetBoxShapeProperties, - gz::physics::GetCapsuleShapeProperties, gz::physics::GetCylinderShapeProperties, - gz::physics::GetSphereShapeProperties, + gz::physics::GetCapsuleShapeProperties, gz::physics::GetEllipsoidShapeProperties, gz::physics::ConstructEmptyWorldFeature, @@ -245,7 +285,7 @@ class ShapeFeaturesTestClass : public ShapeFeaturesTest{}; using ShapeFeaturesTestClassTypes = ::testing::Types; -TYPED_TEST_CASE(ShapeFeaturesTestClass, ShapeFeaturesTestClassTypes); +TYPED_TEST_SUITE(ShapeFeaturesTestClass, ShapeFeaturesTestClassTypes); ///////////////////////////////////////////////// TYPED_TEST(ShapeFeaturesTestClass, ShapeFeatures) @@ -382,8 +422,6 @@ TYPED_TEST(ShapeFeaturesTestClass, ShapeFeatures) EXPECT_TRUE(gz::math::Vector3d(0.2, 0.2, 0.5).Equal( gz::math::eigen3::convert(capsuleAABB).Max(), 0.1)); - -std::cerr << "---------------------------------------" << '\n'; // check model AABB. By default, the AABBs are in world frame auto sphereModelAABB = sphere->GetAxisAlignedBoundingBox(); auto boxModelAABB = box->GetAxisAlignedBoundingBox(); @@ -391,9 +429,6 @@ std::cerr << "---------------------------------------" << '\n'; auto ellipsoidModelAABB = ellipsoid->GetAxisAlignedBoundingBox(); auto capsuleModelAABB = capsule->GetAxisAlignedBoundingBox(); - std::cerr << "gz::math::eigen3::convert(ellipsoidModelAABB) " << gz::math::eigen3::convert(ellipsoidModelAABB).Min() << '\n'; - std::cerr << "gz::math::eigen3::convert(ellipsoidModelAABB) " << gz::math::eigen3::convert(ellipsoidModelAABB).Max() << '\n'; - EXPECT_EQ(gz::math::Vector3d(-1, 0.5, -0.5), gz::math::eigen3::convert(sphereModelAABB).Min()); EXPECT_EQ(gz::math::Vector3d(1, 2.5, 1.5), @@ -439,7 +474,7 @@ TEST_F(SimulationFeaturesTest, FreeGroup) auto freeGroupLink = link->FindFreeGroup(); ASSERT_NE(nullptr, freeGroupLink); - StepWorld(world, true); + StepWorld(world, true); freeGroup->SetWorldPose( gz::math::eigen3::convert( @@ -454,7 +489,7 @@ TEST_F(SimulationFeaturesTest, FreeGroup) gz::math::eigen3::convert(frameData.pose)); // Step the world - StepWorld(world, false); + StepWorld(world, false); // Check that the first link's velocities are updated frameData = model->GetLink(0)->FrameDataRelativeToWorld(); EXPECT_TRUE(gz::math::Vector3d(0.1, 0.2, 0.3).Equal( @@ -478,7 +513,7 @@ TEST_F(SimulationFeaturesTest, CollideBitmasks) auto filteredBox = world->GetModel("box_filtered"); auto collidingBox = world->GetModel("box_colliding"); - auto checkedOutput = StepWorld(world, true); + auto checkedOutput = StepWorld(world, true); EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); // Only box_colliding should collide with box_base @@ -491,7 +526,7 @@ TEST_F(SimulationFeaturesTest, CollideBitmasks) // Also test the getter EXPECT_EQ(0xF0, collidingShape->GetCollisionFilterMask()); // Step and make sure there are no collisions - checkedOutput = StepWorld(world, false); + checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); contacts = world->GetContactsFromLastStep(); EXPECT_EQ(0u, contacts.size()); @@ -500,7 +535,7 @@ TEST_F(SimulationFeaturesTest, CollideBitmasks) // Equivalent to 0xFF collidingShape->RemoveCollisionFilterMask(); filteredShape->RemoveCollisionFilterMask(); - checkedOutput = StepWorld(world, false); + checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); // Expect box_filtered and box_colliding to collide with box_base contacts = world->GetContactsFromLastStep(); @@ -537,7 +572,7 @@ TEST_F(SimulationFeaturesTest, RetrieveContacts) auto box = world->GetModel("box"); // step and get contacts - auto checkedOutput = StepWorld(world, true); + auto checkedOutput = StepWorld(world, true); EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); @@ -596,7 +631,7 @@ TEST_F(SimulationFeaturesTest, RetrieveContacts) gz::math::Pose3d(0, 100, 0.5, 0, 0, 0))); // step and get contacts - checkedOutput = StepWorld(world, false); + checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); contacts = world->GetContactsFromLastStep(); @@ -655,7 +690,7 @@ TEST_F(SimulationFeaturesTest, RetrieveContacts) gz::math::Pose3d(0, -100, -100, 0, 0, 0))); // step and get contacts - checkedOutput = StepWorld(world, false); + checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); contacts = world->GetContactsFromLastStep(); @@ -671,5 +706,9 @@ int main(int argc, char *argv[]) if (!ShapeFeaturesTest::init( argc, argv)) return -1; + + if (!StepWorldTest::init( + argc, argv)) + return -1; return RUN_ALL_TESTS(); } diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc new file mode 100644 index 000000000..ee5acf58b --- /dev/null +++ b/test/common_test/world_features.cc @@ -0,0 +1,140 @@ +/* + * Copyright (C) 2022 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 "../helpers/TestLibLoader.hh" +#include "../Utils.hh" + +#include +#include +#include + +#include +#include + +#include + + +// A predicate-formatter for asserting that two vectors are approximately equal. +class AssertVectorApprox +{ +public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) +{ +} + +public: ::testing::AssertionResult operator()( + const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, + Eigen::Vector3d _n) +{ + if (gz::physics::test::Equal(_m, _n, this->tol)) + return ::testing::AssertionSuccess(); + + return ::testing::AssertionFailure() + << _mExpr << " and " << _nExpr << " ([" << _m.transpose() + << "] and [" << _n.transpose() << "]" + << ") are not equal"; +} + +private: double tol; +}; + + +template +class WorldFeaturesTest: + public testing::Test, public gz::physics::TestLibLoader +{ + // Documentation inherited + public: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + + loader.LoadLib(WorldFeaturesTest::GetLibToTest()); + + // TODO(ahcorde): We should also run the 3f, 2d, and 2f variants of + // FindFeatures + pluginNames = gz::physics::FindFeatures3d::From(loader); + if (pluginNames.empty()) + { + std::cerr << "No plugins with required features found in " + << GetLibToTest() << std::endl; + GTEST_SKIP(); + } + } + + public: std::set pluginNames; + public: gz::plugin::Loader loader; +}; + +using GravityFeatures = gz::physics::FeatureList< + gz::physics::GetEngineInfo, + gz::physics::Gravity, + gz::physics::sdf::ConstructSdfWorld +>; + +template +class GravityFeaturesTestClass : public WorldFeaturesTest{}; +TYPED_TEST_CASE(GravityFeaturesTestClass, GravityFeatures); + +///////////////////////////////////////////////// +TYPED_TEST(GravityFeaturesTestClass, GravityFeatures) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + EXPECT_TRUE(engine->GetName().find(this->PhysicsEngineName(name)) != + std::string::npos); + + sdf::Root root; + const sdf::Errors errors = root.Load( + gz::common::joinPaths(TEST_WORLD_DIR, "test.world")); + EXPECT_TRUE(errors.empty()) << errors; + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + auto graphErrors = sdfWorld->ValidateGraphs(); + EXPECT_EQ(0u, graphErrors.size()) << graphErrors; + + Eigen::Vector3d gravity = {0, 0, -9.8};//= Eigen::Vector3d::Zero(); + + gz::physics::World3dPtr world = + engine->ConstructWorld(*sdfWorld); + EXPECT_NE(nullptr, world); + + AssertVectorApprox vectorPredicate(1e-6); + EXPECT_PRED_FORMAT2(vectorPredicate, gravity, + world->GetGravity()); + + world->SetGravity({8, 4, 3}); + EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d(8, 4, 3), + world->GetGravity()); + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + if(!WorldFeaturesTest::init(argc, argv)) + return -1; + return RUN_ALL_TESTS(); +} diff --git a/test/common_test/worlds/test.world b/test/common_test/worlds/test.world new file mode 100644 index 000000000..b75496857 --- /dev/null +++ b/test/common_test/worlds/test.world @@ -0,0 +1,383 @@ + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 100 100 + + + + 0.9 0.9 0.9 1 + + + + + + 1 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 1 1 0 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 1 1 0 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + 1.1 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 1 1 0 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 1 1 0 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 1 1 0 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + + 0.1 + + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 1 1 0 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 1 1 0 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + 3.0 + + + + + + upper_link + lower_link + + 1.0 0 0 + + 3.0 + + + + + + + 0.0 10.0 10.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 1.0 + + + + + 0.8 + + + + + + + + 10 0 2 0 0 0 + + + 100 + + + 0 0 0 -1.57 0 0 + + + 0.1 + 0.2 + + + + + + 1 0 0 0 0 0 + + 1 + + + + + 0.1 0.1 0.1 + + + + + + world + base + + + -1 0 0 0 0 0 + base + bar + + 0 1 0 + + -0.5 + 0.5 + 100 + + + + + + + 10 10 2 0 0 0 + + + 100 + + + 0 0 0 -1.57 0 0 + + + 0.1 + 0.2 + + + + + + 0 0 -1 0 0 0 + + 1 + + + + + 0.1 0.1 0.1 + + + + + + world + base + + + 0 0 1 0 0 0 + base + bar + + 0 1 0 + + + + + + + + link0 + link1 + 2 + + + + + + + + link0 + link1 + + + + + link2 + link3 + + + + + link4 + link5 + + + + diff --git a/tpe/plugin/worlds/world_with_nested_model.sdf b/test/common_test/worlds/world_with_nested_model.sdf similarity index 63% rename from tpe/plugin/worlds/world_with_nested_model.sdf rename to test/common_test/worlds/world_with_nested_model.sdf index 57653b44e..0b71ad0d7 100644 --- a/tpe/plugin/worlds/world_with_nested_model.sdf +++ b/test/common_test/worlds/world_with_nested_model.sdf @@ -24,8 +24,35 @@ + + nested_link1 + nested_link2 + + 1 0 0 + + - + + + + + 1 2 3 + + + + + + + 1 2 3 + + + + + + + link1 + nested_model::nested_link1 + diff --git a/test/helpers/TestLibLoader.cc b/test/helpers/TestLibLoader.cc index 9a0d8d3c3..e30002b09 100644 --- a/test/helpers/TestLibLoader.cc +++ b/test/helpers/TestLibLoader.cc @@ -54,6 +54,10 @@ namespace physics { std::string physicsEngineName = tokens[2]; std::string physicsEnginePluginName = physicsEngineName; + if (physicsEngineName == "bullet_featherstone") + { + physicsEnginePluginName = "bullet-featherstone"; + } if (physicsEngineName == "tpeplugin") { physicsEnginePluginName = "tpe"; diff --git a/test/integration/FrameSemantics.hh b/test/integration/FrameSemantics.hh index bfe4115c5..57de63556 100644 --- a/test/integration/FrameSemantics.hh +++ b/test/integration/FrameSemantics.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_PHYSICS_TEST_INTEGRATION_FRAMESEMANTICS_HH -#define IGNITION_PHYSICS_TEST_INTEGRATION_FRAMESEMANTICS_HH +#ifndef GZ_PHYSICS_TEST_INTEGRATION_FRAMESEMANTICS_HH +#define GZ_PHYSICS_TEST_INTEGRATION_FRAMESEMANTICS_HH #include #include diff --git a/test/integration/JointTypes.hh b/test/integration/JointTypes.hh index 4e7e03884..d658604c9 100644 --- a/test/integration/JointTypes.hh +++ b/test/integration/JointTypes.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_PHYSICS_TEST_INGEGRATION_JOINTTYPES_HH -#define IGNITION_PHYSICS_TEST_INGEGRATION_JOINTTYPES_HH +#ifndef GZ_PHYSICS_TEST_INGEGRATION_JOINTTYPES_HH +#define GZ_PHYSICS_TEST_INGEGRATION_JOINTTYPES_HH #include #include diff --git a/test/performance/CMakeLists.txt b/test/performance/CMakeLists.txt index 2646e63da..1034620fa 100644 --- a/test/performance/CMakeLists.txt +++ b/test/performance/CMakeLists.txt @@ -1,7 +1,7 @@ gz_get_sources(tests) # ExpectData test causes lcov to hang -# see ign-cmake issue 25 +# see gz-cmake issue 25 if("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "COVERAGE") list(REMOVE_ITEM tests ExpectData.cc) diff --git a/tpe/CMakeLists.txt b/tpe/CMakeLists.txt index a66e7c3e1..a77cfeeff 100644 --- a/tpe/CMakeLists.txt +++ b/tpe/CMakeLists.txt @@ -3,4 +3,4 @@ add_subdirectory(plugin) install( DIRECTORY include/ - DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}") + DESTINATION "${GZ_INCLUDE_INSTALL_DIR_FULL}") diff --git a/tpe/plugin/CMakeLists.txt b/tpe/plugin/CMakeLists.txt index bcbf58be5..f3075af5b 100644 --- a/tpe/plugin/CMakeLists.txt +++ b/tpe/plugin/CMakeLists.txt @@ -9,7 +9,7 @@ target_include_directories(${features} SYSTEM INTERFACE) gz_get_libsources_and_unittests(sources test_sources) -# TODO(MXG): Think about an gz_add_plugin(~) macro for ign-cmake +# TODO(MXG): Think about a gz_add_plugin(~) macro for gz-cmake set(engine_name tpe-plugin) gz_add_component(${engine_name} SOURCES ${sources} diff --git a/tpe/plugin/src/FreeGroupFeatures_TEST.cc b/tpe/plugin/src/FreeGroupFeatures_TEST.cc deleted file mode 100644 index df255d453..000000000 --- a/tpe/plugin/src/FreeGroupFeatures_TEST.cc +++ /dev/null @@ -1,194 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include "gz/physics/FrameSemantics.hh" -#include "gz/physics/FreeGroup.hh" -#include "gz/physics/GetEntities.hh" -#include "gz/physics/RequestEngine.hh" -#include "gz/physics/sdf/ConstructModel.hh" -#include "gz/physics/sdf/ConstructNestedModel.hh" -#include "gz/physics/sdf/ConstructWorld.hh" -#include "test/Utils.hh" - -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::sdf::ConstructSdfWorld, - gz::physics::sdf::ConstructSdfModel, - gz::physics::sdf::ConstructSdfNestedModel, - gz::physics::GetWorldFromEngine, - gz::physics::GetModelFromWorld, - gz::physics::GetNestedModelFromModel, - gz::physics::GetLinkFromModel, - gz::physics::LinkFrameSemantics, - gz::physics::FindFreeGroupFeature, - gz::physics::SetFreeGroupWorldPose > { }; - -using World = gz::physics::World3d; -using WorldPtr = gz::physics::World3dPtr; -using ModelPtr = gz::physics::Model3dPtr; -using LinkPtr = gz::physics::Link3dPtr; - -///////////////////////////////////////////////// -auto LoadEngine() -{ - gz::plugin::Loader loader; - loader.LoadLib(tpe_plugin_LIB); - - gz::plugin::PluginPtr tpe = - loader.Instantiate("gz::physics::tpeplugin::Plugin"); - - auto engine = - gz::physics::RequestEngine3d::From(tpe); - - return engine; -} - -///////////////////////////////////////////////// -WorldPtr LoadWorld(const std::string &_world) -{ - auto engine = LoadEngine(); - EXPECT_NE(nullptr, engine); - if (nullptr == engine) - { - return nullptr; - } - - sdf::Root root; - const sdf::Errors &errors = root.Load(_world); - EXPECT_EQ(0u, errors.size()) << errors; - - EXPECT_EQ(1u, root.WorldCount()); - const sdf::World *sdfWorld = root.WorldByIndex(0); - EXPECT_NE(nullptr, sdfWorld); - - auto world = engine->ConstructWorld(*sdfWorld); - EXPECT_NE(nullptr, world); - - return world; -} - -ModelPtr GetModelFromAbsoluteName(const WorldPtr &_world, - const std::string &_absoluteName) -{ - std::vector names = - gz::common::split(_absoluteName, sdf::kSdfScopeDelimiter); - if (names.empty()) - { - return nullptr; - } - - auto currentModel = _world->GetModel(names.front()); - for (std::size_t i = 1; i < names.size(); ++i) - { - if (nullptr == currentModel) - return nullptr; - currentModel = currentModel->GetNestedModel(names[i]); - } - return currentModel; -} - -TEST(FreeGroupFeatures, NestedFreeGroup) -{ - WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); - ASSERT_NE(nullptr, world); - - auto checkFreeGroupForModel = [&](const std::string &_modelName) - { - auto model = GetModelFromAbsoluteName(world, _modelName); - if (nullptr == model) - return testing::AssertionFailure() << "Model could not be found"; - auto freeGroup = model->FindFreeGroup(); - if (nullptr == freeGroup) - return testing::AssertionFailure() << "Freegroup could not be found"; - if (nullptr == freeGroup->RootLink()) - return testing::AssertionFailure() << "RootLink could not be found"; - return testing::AssertionSuccess(); - }; - - EXPECT_TRUE(checkFreeGroupForModel("parent_model")); - EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model")); - EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model2")); - EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model3")); -} - -TEST(FreeGroupFeatures, NestedFreeGroupSetWorldPose) -{ - WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); - ASSERT_NE(nullptr, world); - - gz::math::Pose3d parentModelNewPose(0, 0, 2, 0, 0, 0); - { - auto parentModel = world->GetModel("parent_model"); - ASSERT_NE(nullptr, parentModel); - auto freeGroup = parentModel->FindFreeGroup(); - ASSERT_NE(nullptr, freeGroup); - - freeGroup->SetWorldPose( - gz::math::eigen3::convert(parentModelNewPose)); - - auto link1 = parentModel->GetLink("link1"); - ASSERT_NE(nullptr, link1); - auto frameData = link1->FrameDataRelativeToWorld(); - EXPECT_EQ(parentModelNewPose, - gz::math::eigen3::convert(frameData.pose)); - } - { - auto nestedModel = - GetModelFromAbsoluteName(world, "parent_model::nested_model"); - ASSERT_NE(nullptr, nestedModel); - auto nestedLink1 = nestedModel->GetLink("nested_link1"); - ASSERT_NE(nullptr, nestedLink1); - auto frameData = nestedLink1->FrameDataRelativeToWorld(); - // Poses from SDF - gz::math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0); - gz::math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, GZ_PI_2); - - gz::math::Pose3d nestedLinkExpectedPose = - parentModelNewPose * nestedModelPose * nestedLinkPose; - - EXPECT_EQ(nestedLinkExpectedPose, - gz::math::eigen3::convert(frameData.pose)); - } - { - auto parentModel = world->GetModel("parent_model2"); - ASSERT_NE(nullptr, parentModel); - auto freeGroup = parentModel->FindFreeGroup(); - ASSERT_NE(nullptr, freeGroup); - - freeGroup->SetWorldPose( - gz::math::eigen3::convert(parentModelNewPose)); - - auto grandChild = GetModelFromAbsoluteName( - world, "parent_model2::child_model::grand_child_model"); - ASSERT_NE(nullptr, grandChild); - auto link1 = grandChild->GetLink("link1"); - ASSERT_NE(nullptr, link1); - auto frameData = link1->FrameDataRelativeToWorld(); - EXPECT_EQ(parentModelNewPose, - gz::math::eigen3::convert(frameData.pose)); - } -} diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index 967ef95a8..409e222c7 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -321,7 +321,7 @@ Identity SDFFeatures::ConstructSdfCollision( } // \todo(anyone) add mesh. currently mesh has to be loaded externally // and passed in as argument as there is no logic for searching resources - // in ign-physics + // in gz-physics const auto collisionIdentity = this->AddCollision(link->GetId(), *collision); // set collide bitmask diff --git a/tutorials.md.in b/tutorials.md.in index e69f8c3a0..d0ef00a9a 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -1,8 +1,8 @@ \page tutorials Tutorials -Welcome to the Gazebo @IGN_DESIGNATION_CAP@ tutorials. These tutorials +Welcome to the Gazebo @GZ_DESIGNATION_CAP@ tutorials. These tutorials will guide you through the process of understanding the capabilities of the -Gazebo @IGN_DESIGNATION_CAP@ library and how to use the library effectively. +Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. **Contents** diff --git a/tutorials/01_intro.md b/tutorials/01_intro.md index 425f31571..5ae8eddd2 100644 --- a/tutorials/01_intro.md +++ b/tutorials/01_intro.md @@ -20,7 +20,7 @@ Gazebo Physics extensibility and modularity. For a big picture of the Gazebo Physics operation in Gazebo ecosystem, see the abstract diagram below: -@image html img/ign-libraries.png +@image html img/gz-libraries.png In general, `gz-sim` is the main simulation library, in which its functionalities are powered by many component libraries. diff --git a/tutorials/02_installation.md b/tutorials/02_installation.md index 478ce2d55..5de68e93f 100644 --- a/tutorials/02_installation.md +++ b/tutorials/02_installation.md @@ -35,7 +35,7 @@ wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - On Ubuntu systems, `apt-get` can be used to install `gz-plugin`: ``` sudo apt-get update -sudo apt-get install libignition-physics<#>-dev +sudo apt-get install libgz-physics<#>-dev ``` Be sure to replace `<#>` with a number value, such as `1` or `2`, depending on which version you need. @@ -44,7 +44,7 @@ Be sure to replace `<#>` with a number value, such as `1` or `2`, depending on w 1. Install dependencies ``` sudo apt-add-repository -s "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -c -s) main" - sudo apt-get build-dep -y ignition-physics<#>-dev + sudo apt-get build-dep -y gz-physics<#>-dev ``` Be sure to replace `<#>` with a number value, such as `1` or `2`, depending on which version you need. @@ -90,7 +90,7 @@ command-line tools: 2. Run the following commands ``` brew tap osrf/simulation - brew install ignition-physics<#> + brew install gz-physics<#> ``` Be sure to replace `<#>` with a number value, such as `1` or `2`, depending on which version you need. @@ -99,7 +99,7 @@ Be sure to replace `<#>` with a number value, such as `1` or `2`, depending on w 1. Install dependencies ``` brew tap osrf/simulation - brew install ignition-physics<#> --only-dependencies + brew install gz-physics<#> --only-dependencies ``` 2. Clone the repository @@ -142,7 +142,7 @@ conda activate gz-ws ## Binary Installation ``` -conda install libignition-physics<#> --channel conda-forge +conda install libgz-physics<#> --channel conda-forge ``` Be sure to replace `<#>` with a number value, such as 1 or 2, depending on @@ -156,12 +156,12 @@ This assumes you have created and activated a Conda environment while installing You can view available versions and their dependencies: ``` - conda search libignition-physics* --channel conda-forge --info + conda search libgz-physics* --channel conda-forge --info ``` Install dependencies, replacing `<#>` with the desired versions: ``` - conda install libignition-cmake<#> libignition-common<#> libignition-math<#> libignition-plugin<#> libsdformat<#> --channel conda-forge + conda install libgz-cmake<#> libgz-common<#> libgz-math<#> libgz-plugin<#> libsdformat<#> --channel conda-forge ``` 2. Navigate to where you would like to build the library, and clone the repository. diff --git a/tutorials/06-physics-simulation-concepts.md b/tutorials/06-physics-simulation-concepts.md index 7c74e58b3..1e7372002 100644 --- a/tutorials/06-physics-simulation-concepts.md +++ b/tutorials/06-physics-simulation-concepts.md @@ -10,7 +10,7 @@ This tutorial introduces simulation concepts that are used in Gazebo Physics. ## Physics simulation features -Here is a snapshot of simulation features supported by ign-physics: +Here is a snapshot of simulation features supported by gz-physics: - Collision detection: ODE, Bullet, various collision shapes and mesh - Kinematics; joint, arbitrary body shapes, various kinematic states like transmoration, velocity, acceleration etc., inverse kinematics @@ -56,7 +56,7 @@ To control the car movement, in a separate terminal window, we publish a Twist message using Gazebo Transport library: ```bash -ign topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 1.0}, angular: {z: 0.5}" +gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 1.0}, angular: {z: 0.5}" ``` Then press the Play button to start the simulation. @@ -114,13 +114,13 @@ Double message represeting the torque (Nm) applying to the rotor rod axis: ```bash -ign topic -t "/model/lift_drag_demo_model/joint/rod_1_joint/cmd_force" -m gz.msgs.Double -p "data: 0.7" +gz topic -t "/model/lift_drag_demo_model/joint/rod_1_joint/cmd_force" -m gz.msgs.Double -p "data: 0.7" ``` Then please press Play button to start the simulation. ```bash -ign topic -t "/model/lift_drag_demo_model/joint/rod_1_joint/cmd_force" -m gz.msgs.Double -p "data: 0.0" +gz topic -t "/model/lift_drag_demo_model/joint/rod_1_joint/cmd_force" -m gz.msgs.Double -p "data: 0.0" ``` You will see the cube drops due to no lift force from support torque on the rod, @@ -209,13 +209,13 @@ Twist message to command the `X3` multicopter ascending 0.1 m.s as follow: ```bash -ign topic -t "/X3/gazebo/command/twist" -m gz.msgs.Twist -p "linear: {x:0 y: 0 z: 0.1} angular {z: 0}" +gz topic -t "/X3/gazebo/command/twist" -m gz.msgs.Twist -p "linear: {x:0 y: 0 z: 0.1} angular {z: 0}" ``` then hovering: ```bash -ign topic -t "/X3/gazebo/command/twist" -m gz.msgs.Twist -p " " +gz topic -t "/X3/gazebo/command/twist" -m gz.msgs.Twist -p " " ``` @image html img/hover.gif width=100% diff --git a/tutorials/08-implementing-a-custom-feature.md b/tutorials/08-implementing-a-custom-feature.md index e4968ef16..7db6fd8ea 100644 --- a/tutorials/08-implementing-a-custom-feature.md +++ b/tutorials/08-implementing-a-custom-feature.md @@ -22,11 +22,11 @@ physics engine in `gz-physics` repository and how to define a custom Below is the general structure of the `gz-physics` repository: ``` -ign-physics +gz-physics ├── dartsim Files for dartsim plugin component. ├── tpe Files for tpe plugin component. ├── heightmap Files for heightmap component. -├── include/ignition/physics Header files. +├── include/gz/physics Header files. ├── mesh Files for mesh component. ├── resources Model and mesh resource files used by tests. ├── sdf Files for sdf component. @@ -49,14 +49,14 @@ Looking closer to a plugin folder, for example, the `dartsim` (DART) plugin: dartsim ├── worlds Example SDF files for testing dartsim plugin functionalities. ├── src Main implementation files of the plugin features interfacing the physics engines API -├── include/ignition/physics/dartsim Header files for the plugin features. +├── include/gz/physics/dartsim Header files for the plugin features. └── CMakeLists.txt CMake plugin build script. ``` Basically, new implementation of \ref gz::physics::Feature "Feature" or \ref gz::physics::FeatureList "FeatureList", which is corresponded to a functionality of the external physics engine can be defined as a header in -`include/ignition/physics/` folder. The custom feature could +`include/gz/physics/` folder. The custom feature could be added in a \ref gz::physics::FeatureList "FeatureList" and implemented its functionalities in `src` folder. @@ -134,9 +134,9 @@ will not run at the same time when requested. ### Define custom feature template -With the requirements and restrictions above, we first need to define a feature template for the custom feature. In this case, this feature will be responsible for retrieving world pointer from the physics engine. The template is placed in [World.hh](https://github.com/gazebosim/gz-physics/blob/main/dartsim/include/ignition/physics/dartsim/World.hh): +With the requirements and restrictions above, we first need to define a feature template for the custom feature. In this case, this feature will be responsible for retrieving world pointer from the physics engine. The template is placed in [World.hh](https://github.com/gazebosim/gz-physics/blob/main/dartsim/include/gz/physics/dartsim/World.hh): -\snippet dartsim/include/ignition/physics/dartsim/World.hh feature template +\snippet dartsim/include/gz/physics/dartsim/World.hh feature template The `RetrieveWorld` feature retrieves world pointer from physics engine, so we will use the `World` entity inherited @@ -160,7 +160,7 @@ dartsim │ ├── CustomFeatures.hh │ ├── CustomFeatures.cc │ └── ... -├── include/ignition/physics/dartsim +├── include/gz/physics/dartsim │ └── World.hh └── CMakeLists.txt ``` @@ -208,6 +208,6 @@ dartsim │ ├── CustomFeatures.hh │ ├── CustomFeatures.cc │ ├── ... -├── include/ignition/physics/dartsim +├── include/gz/physics/dartsim └── CMakeLists.txt ``` diff --git a/tutorials/09-set-up-physics-engine-tpe.md b/tutorials/09-set-up-physics-engine-tpe.md index 16e9d3d17..5c3ea1805 100644 --- a/tutorials/09-set-up-physics-engine-tpe.md +++ b/tutorials/09-set-up-physics-engine-tpe.md @@ -127,8 +127,8 @@ and [Base.hh](https://github.com/gazebosim/gz-physics/blob/main/tpe/plugin/src/B into `plugin` folder by: ```bash -wget https://raw.githubusercontent.com/gazebosim/gz-physics/main/tpe/plugin/CMakeLists.txt -P /tpe/plugin/ -wget https://raw.githubusercontent.com/gazebosim/gz-physics/main/tpe/plugin/src/Base.hh -P /tpe/plugin/src +wget https://raw.githubusercontent.com/gazebosim/gz-physics/main/tpe/plugin/CMakeLists.txt -P /tpe/plugin/ +wget https://raw.githubusercontent.com/gazebosim/gz-physics/main/tpe/plugin/src/Base.hh -P /tpe/plugin/src ``` Now the folder structure looks like this: diff --git a/tutorials/09_use_custom_engine.md b/tutorials/09_use_custom_engine.md index 900b5ed07..7eeaa1b07 100644 --- a/tutorials/09_use_custom_engine.md +++ b/tutorials/09_use_custom_engine.md @@ -23,7 +23,7 @@ the folder structure could be slightly different from what's shown below. Here's the plugin folder structure of TPE, within the Gazebo Physics library. ``` -ign-physics +gz-physics ├── tpe │ ├── plugin Implementation of the plugin features interfacing the physics engines API │ │ ├── src @@ -103,7 +103,7 @@ in `EntityManagementFeatures` "FeatureList" using TPE API from `tpe/lib` in Gaze Before we dive into the feature implementation, we need to understand how the features are defined. The \ref gz::physics::ConstructEmptyWorldFeature "ConstructEmptyWorldFeature" -is declared in a function template file `gz-physics/include/ignition/physics/ConstructEmpty.hh`. +is declared in a function template file `gz-physics/include/gz/physics/ConstructEmpty.hh`. Gazebo Physics library uses function templates to specify features that accept generic types. The use of templates makes it easier to implement features using different physics engine APIs, diff --git a/tutorials/img/ign-libraries.png b/tutorials/img/gz-libraries.png similarity index 100% rename from tutorials/img/ign-libraries.png rename to tutorials/img/gz-libraries.png