diff --git a/examples/plugin/hello_world/CMakeLists.txt b/examples/plugin/hello_world/CMakeLists.txt new file mode 100644 index 0000000000..6d6213c2c7 --- /dev/null +++ b/examples/plugin/hello_world/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-cmake2 REQUIRED) + +project(Hello_world) + +ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) +set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + +ign_find_package(ignition-gazebo5 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) + +add_library(HelloWorld SHARED HelloWorld) +set_property(TARGET HelloWorld PROPERTY CXX_STANDARD 17) +target_link_libraries(HelloWorld + PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}) diff --git a/examples/plugin/hello_world/HelloWorld.cc b/examples/plugin/hello_world/HelloWorld.cc new file mode 100644 index 0000000000..d73ed28b6c --- /dev/null +++ b/examples/plugin/hello_world/HelloWorld.cc @@ -0,0 +1,56 @@ +/* + * 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. + * +*/ + +// We'll use a string and the ignmsg command below for a brief example. +// Remove these includes if your plugin doesn't need them. +#include +#include + +// This header is required to register plugins. It's good practice to place it +// in the cc file, like it's done here. +#include + +// Don't forget to include the plugin's header. +#include "HelloWorld.hh" + +// This is required to register the plugin. Make sure the interfaces match +// what's in the header. +IGNITION_ADD_PLUGIN( + hello_world::HelloWorld, + ignition::gazebo::System, + hello_world::HelloWorld::ISystemPostUpdate) + +using namespace hello_world; + +// Here we implement the PostUpdate function, which is called at every +// iteration. +void HelloWorld::PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + // This is a simple example of how to get information from UpdateInfo. + std::string msg = "Hello, world! Simulation is "; + if (!_info.paused) + msg += "not "; + msg += "paused."; + + // Messages printed with ignmsg only show when running with verbosity 3 or + // higher (i.e. ign gazebo -v 3) + ignmsg << msg << std::endl; +} + + + diff --git a/examples/plugin/hello_world/HelloWorld.hh b/examples/plugin/hello_world/HelloWorld.hh new file mode 100644 index 0000000000..ff60e09392 --- /dev/null +++ b/examples/plugin/hello_world/HelloWorld.hh @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SYSTEM_PLUGIN_HELLOWORLD_HH_ +#define SYSTEM_PLUGIN_HELLOWORLD_HH_ + +// The only required include in the header is this one. +// All others will depend on what your plugin does. +#include + +// It's good practice to use a custom namespace for your project. +namespace hello_world +{ + // This is the main plugin's class. It must inherit from System and at least + // one other interface. + // Here we use `ISystemPostUpdate`, which is used to get results after + // physics runs. The opposite of that, `ISystemPreUpdate`, would be used by + // plugins that want to send commands. + class HelloWorld: + public ignition::gazebo::System, + public ignition::gazebo::ISystemPostUpdate + { + // Plugins inheriting ISystemPostUpdate must implement the PostUpdate + // callback. This is called at every simulation iteration after the physics + // updates the world. The _info variable provides information such as time, + // while the _ecm provides an interface to all entities and components in + // simulation. + public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; + }; +} +#endif diff --git a/examples/plugin/hello_world/README.md b/examples/plugin/hello_world/README.md new file mode 100644 index 0000000000..75422b8f93 --- /dev/null +++ b/examples/plugin/hello_world/README.md @@ -0,0 +1,42 @@ +# Hello world + +This example contains the bare minimum that's necessary to create a Gazebo +system plugin. + +## Build + +From the root of the `ign-gazebo` repository, do the following to build the example: + +~~~ +cd ign-gazebo/examples/plugins/hello_world +mkdir build +cd build +cmake .. +make +~~~ + +This will generate the `HelloWorld` library under `build`. + +## Run + +The plugin must be attached to an entity to be loaded. This is demonstrated in +the `hello_world_plugin.sdf` file that's going to be loaded. + +Before starting Gazebo, we must make sure it can find the plugin by doing: + +~~~ +cd ign-gazebo/examples/plugins/hello_world +export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/build +~~~ + +Then load the example world: + + ign gazebo -v 3 hello_world_plugin.sdf + +You should see green messages on the terminal like: + +``` +[Msg] Hello, world! Simulation is paused. +``` + +Toggle the play / pause buttons to see the message change. diff --git a/examples/plugin/hello_world/hello_world_plugin.sdf b/examples/plugin/hello_world/hello_world_plugin.sdf new file mode 100644 index 0000000000..b304e03302 --- /dev/null +++ b/examples/plugin/hello_world/hello_world_plugin.sdf @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/include/ignition/gazebo/Model.hh b/include/ignition/gazebo/Model.hh index 89450cb45c..b79b5ec055 100644 --- a/include/ignition/gazebo/Model.hh +++ b/include/ignition/gazebo/Model.hh @@ -151,6 +151,12 @@ namespace ignition public: std::vector Links( const EntityComponentManager &_ecm) const; + /// \brief Get all models which are immediate children of this model. + /// \param[in] _ecm Entity-component manager. + /// \return All models in this model. + public: std::vector Models( + const EntityComponentManager &_ecm) const; + /// \brief Get the number of joints which are immediate children of this /// model. /// \param[in] _ecm Entity-component manager. diff --git a/include/ignition/gazebo/Types.hh b/include/ignition/gazebo/Types.hh index 2280cdb751..84026a0c9e 100644 --- a/include/ignition/gazebo/Types.hh +++ b/include/ignition/gazebo/Types.hh @@ -97,7 +97,7 @@ namespace ignition static const ComponentId kComponentIdInvalid = -1; /// \brief Id that indicates an invalid component type. - static const ComponentTypeId kComponentTypeIdInvalid = -1; + static const ComponentTypeId kComponentTypeIdInvalid = UINT64_MAX; } } } diff --git a/include/ignition/gazebo/components/Sensor.hh b/include/ignition/gazebo/components/Sensor.hh index 97ca8d74a0..2d7ddb39a3 100644 --- a/include/ignition/gazebo/components/Sensor.hh +++ b/include/ignition/gazebo/components/Sensor.hh @@ -31,7 +31,7 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief A component that identifies an entity as being a link. + /// \brief A component that identifies an entity as being a sensor. using Sensor = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Sensor", Sensor) diff --git a/src/Model.cc b/src/Model.cc index c7e9877577..2dfa533e51 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -163,6 +163,14 @@ std::vector Model::Links(const EntityComponentManager &_ecm) const components::Link()); } +////////////////////////////////////////////////// +std::vector Model::Models(const EntityComponentManager &_ecm) const +{ + return _ecm.EntitiesByComponents( + components::ParentEntity(this->dataPtr->id), + components::Model()); +} + ////////////////////////////////////////////////// uint64_t Model::JointCount(const EntityComponentManager &_ecm) const { diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index e6134f6860..81304e2e16 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -17,7 +17,14 @@ #include +#include + +#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" ///////////////////////////////////////////////// TEST(ModelTest, Constructor) @@ -74,3 +81,145 @@ TEST(ModelTest, MoveAssignmentOperator) modelMoved = std::move(model); EXPECT_EQ(id, modelMoved.Entity()); } + +///////////////////////////////////////////////// +TEST(ModelTest, Links) +{ + // modelA + // - linkAA + // - linkAB + // - modelB + // - linkBA + // + // modelC + + ignition::gazebo::EntityComponentManager ecm; + + // Model A + auto modelAEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelAEntity, ignition::gazebo::components::Model()); + ecm.CreateComponent(modelAEntity, + ignition::gazebo::components::Name("modelA_name")); + + // Link AA - Child of Model A + auto linkAAEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkAAEntity, ignition::gazebo::components::Link()); + ecm.CreateComponent(linkAAEntity, + ignition::gazebo::components::Name("linkAA_name")); + ecm.CreateComponent(linkAAEntity, + ignition::gazebo::components::ParentEntity(modelAEntity)); + + // Link AB - Child of Model A + auto linkABEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkABEntity, ignition::gazebo::components::Link()); + ecm.CreateComponent(linkABEntity, + ignition::gazebo::components::Name("linkAB_name")); + ecm.CreateComponent(linkABEntity, + ignition::gazebo::components::ParentEntity(modelAEntity)); + + // Model B - Child of Model A + auto modelBEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelBEntity, ignition::gazebo::components::Model()); + ecm.CreateComponent(modelBEntity, + ignition::gazebo::components::Name("modelB_name")); + ecm.CreateComponent(modelBEntity, + ignition::gazebo::components::ParentEntity(modelAEntity)); + + // Link BA - Child of Model B + auto linkBAEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkBAEntity, ignition::gazebo::components::Link()); + ecm.CreateComponent(linkBAEntity, + ignition::gazebo::components::Name("linkBA_name")); + ecm.CreateComponent(linkBAEntity, + ignition::gazebo::components::ParentEntity(modelBEntity)); + + // Model C + auto modelCEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelCEntity, ignition::gazebo::components::Model()); + ecm.CreateComponent(modelCEntity, + ignition::gazebo::components::Name("modelC_name")); + + std::size_t foundLinks = 0; + + ignition::gazebo::Model modelA(modelAEntity); + auto links = modelA.Links(ecm); + EXPECT_EQ(2u, links.size()); + for (const auto &link : links) + { + if (link == linkAAEntity || link == linkABEntity) + foundLinks++; + } + EXPECT_EQ(foundLinks, links.size()); + + ignition::gazebo::Model modelB(modelBEntity); + links = modelB.Links(ecm); + ASSERT_EQ(1u, links.size()); + EXPECT_EQ(linkBAEntity, links[0]); + + ignition::gazebo::Model modelC(modelCEntity); + EXPECT_EQ(0u, modelC.Links(ecm).size()); +} + +///////////////////////////////////////////////// +TEST(ModelTest, Models) +{ + // modelA + // - modelB + // - modelC + // - modelD + + ignition::gazebo::EntityComponentManager ecm; + + // Model A + auto modelAEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelAEntity, ignition::gazebo::components::Model()); + ecm.CreateComponent(modelAEntity, + ignition::gazebo::components::Name("modelA_name")); + + // Model B - Child of Model A + auto modelBEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelBEntity, ignition::gazebo::components::Model()); + ecm.CreateComponent(modelBEntity, + ignition::gazebo::components::Name("modelB_name")); + ecm.CreateComponent(modelBEntity, + ignition::gazebo::components::ParentEntity(modelAEntity)); + + // Model C - Child of Model A + auto modelCEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelCEntity, ignition::gazebo::components::Model()); + ecm.CreateComponent(modelCEntity, + ignition::gazebo::components::Name("modelC_name")); + ecm.CreateComponent(modelCEntity, + ignition::gazebo::components::ParentEntity(modelAEntity)); + + // Model D - Child of Model C + auto modelDEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelDEntity, ignition::gazebo::components::Model()); + ecm.CreateComponent(modelDEntity, + ignition::gazebo::components::Name("modelD_name")); + ecm.CreateComponent(modelDEntity, + ignition::gazebo::components::ParentEntity(modelCEntity)); + + std::size_t foundModels = 0; + + ignition::gazebo::Model modelA(modelAEntity); + auto models = modelA.Models(ecm); + EXPECT_EQ(2u, models.size()); + for (const auto &model : models) + { + if (model == modelBEntity || model == modelCEntity) + foundModels++; + } + EXPECT_EQ(foundModels, models.size()); + + ignition::gazebo::Model modelB(modelBEntity); + EXPECT_EQ(0u, modelB.Models(ecm).size()); + + ignition::gazebo::Model modelC(modelCEntity); + models = modelC.Models(ecm); + ASSERT_EQ(1u, models.size()); + EXPECT_EQ(modelDEntity, models[0]); + + ignition::gazebo::Model modelD(modelDEntity); + EXPECT_EQ(0u, modelD.Models(ecm).size()); +} diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index f1692a63da..cf74a3246c 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -257,7 +257,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::RayQueryPtr rayQuery; /// \brief View control focus target - public: math::Vector3d target; + public: math::Vector3d target = math::Vector3d( + math::INF_D, math::INF_D, math::INF_D); /// \brief Rendering utility public: RenderUtil renderUtil; @@ -1810,12 +1811,24 @@ void IgnRenderer::HandleMouseViewControl() } else { - if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS) + if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS || + // the rendering thread may miss the press event due to + // race condition when doing a drag operation (press and move, where + // the move event overrides the press event before it is processed) + // so we double check to see if target is set or not + (this->dataPtr->mouseEvent.Type() == common::MouseEvent::MOVE && + this->dataPtr->mouseEvent.Dragging() && + std::isinf(this->dataPtr->target.X()))) { this->dataPtr->target = this->ScreenToScene( this->dataPtr->mouseEvent.PressPos()); this->dataPtr->viewControl.SetTarget(this->dataPtr->target); } + // unset the target on release (by setting to inf) + else if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + this->dataPtr->target = ignition::math::INF_D; + } // Pan with left button if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::LEFT) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index be216c1a48..fc89c760f2 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1946,7 +1946,8 @@ void RenderUtilPrivate::RemoveSensor(const Entity _entity) auto sensorEntityIt = this->sensorEntities.find(_entity); if (sensorEntityIt != this->sensorEntities.end()) { - this->removeSensorCb(_entity); + if (this->removeSensorCb) + this->removeSensorCb(_entity); this->sensorEntities.erase(sensorEntityIt); } } diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 1df398e07e..2ff0d5b110 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -529,8 +529,7 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, // Construct the Pose_V/tf message and publish it. msgs::Pose_V tfMsg; - ignition::msgs::Pose *tfMsgPose = nullptr; - tfMsgPose = tfMsg.add_pose(); + ignition::msgs::Pose *tfMsgPose = tfMsg.add_pose(); tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index b496aaea27..f51d04a6d5 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -78,6 +78,7 @@ #include #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" // Components @@ -205,14 +206,36 @@ class ignition::gazebo::systems::PhysicsPrivate EntityComponentManager &_ecm, const ignition::physics::ForwardStep::Output &_updatedLinks); + /// \brief Helper function to update the pose of a model. + /// \param[in] _model The model to update. + /// \param[in] _canonicalLink The canonical link of _model. + /// \param[in] _ecm The entity component manager. + /// \param[in, out] _linkFrameData Links that experienced a pose change in the + /// most recent physics step. The key is the entity of the link, and the + /// value is the updated frame data corresponding to that entity. The + /// canonical links of _model's nested models are added to _linkFrameData to + /// ensure that all of _model's nested models are marked as models to be + /// updated (if a parent model's pose changes, all nested model poses must be + /// updated since nested model poses are saved w.r.t. the parent model). + public: void UpdateModelPose(const Entity _model, + const Entity _canonicalLink, EntityComponentManager &_ecm, + std::map &_linkFrameData); + + /// \brief Get an entity's frame data relative to world from physics. + /// \param[in] _entity The entity. + /// \param[in, out] _data The frame data to populate. + /// \return True if _data was populated with frame data for _entity, false + /// otherwise. + public: bool GetFrameDataRelativeToWorld(const Entity _entity, + physics::FrameData3d &_data); + /// \brief Update components from physics simulation /// \param[in] _ecm Mutable reference to ECM. - /// \param[in] _linkFrameData Links that experienced a pose change in the + /// \param[in, out] _linkFrameData Links that experienced a pose change in the /// most recent physics step. The key is the entity of the link, and the /// value is the updated frame data corresponding to that entity. public: void UpdateSim(EntityComponentManager &_ecm, - const std::map< - Entity, physics::FrameData3d> &_linkFrameData); + std::map &_linkFrameData); /// \brief Update collision components from physics simulation /// \param[in] _ecm Mutable reference to ECM. @@ -2041,9 +2064,138 @@ std::map PhysicsPrivate::ChangedLinks( return linkFrameData; } +////////////////////////////////////////////////// +void PhysicsPrivate::UpdateModelPose(const Entity _model, + const Entity _canonicalLink, EntityComponentManager &_ecm, + std::map &_linkFrameData) +{ + std::optional parentWorldPose; + + // If this model is nested, the pose of the parent model has already + // been updated since we iterate through the modified links in + // topological order. We expect to find the updated pose in + // this->modelWorldPoses. If not found, this must not be nested, so this + // model's pose component would reflect it's absolute pose. + auto parentModelPoseIt = + this->modelWorldPoses.find( + _ecm.Component(_model)->Data()); + if (parentModelPoseIt != this->modelWorldPoses.end()) + { + parentWorldPose = parentModelPoseIt->second; + } + + // Given the following frame names: + // W: World/inertial frame + // P: Parent frame (this could be a parent model or the World frame) + // M: This model's frame + // L: The frame of this model's canonical link + // + // And the following quantities: + // (See http://sdformat.org/tutorials?tut=specify_pose for pose + // convention) + // parentWorldPose (X_WP): Pose of the parent frame w.r.t the world + // linkPoseFromModel (X_ML): Pose of the canonical link frame w.r.t the + // model frame + // linkWorldPose (X_WL): Pose of the canonical link w.r.t the world + // modelWorldPose (X_WM): Pose of this model w.r.t the world + // + // The Pose component of this model entity stores the pose of M w.r.t P + // (X_PM) and is calculated as + // X_PM = (X_WP)^-1 * X_WM + // + // And X_WM is calculated from X_WL, which is obtained from physics as: + // X_WM = X_WL * (X_ML)^-1 + auto linkPoseFromModel = this->RelativePose(_model, _canonicalLink, _ecm); + const auto &linkWorldPose = _linkFrameData[_canonicalLink].pose; + const auto &modelWorldPose = + math::eigen3::convert(linkWorldPose) * linkPoseFromModel.Inverse(); + + this->modelWorldPoses[_model] = modelWorldPose; + + // update model's pose + auto modelPose = _ecm.Component(_model); + if (parentWorldPose) + { + *modelPose = + components::Pose(parentWorldPose->Inverse() * modelWorldPose); + } + else + { + // This is a non-nested model and parentWorldPose would be identity + // because it would be the pose of the parent (world) w.r.t the world. + *modelPose = components::Pose(modelWorldPose); + } + + _ecm.SetChanged(_model, components::Pose::typeId, + ComponentState::PeriodicChange); + + // once the model pose has been updated, all descendant link poses of this + // model must be updated (whether the link actually changed pose or not) + // since link poses are saved w.r.t. their parent model + auto model = gazebo::Model(_model); + for (const auto &childLink : model.Links(_ecm)) + { + // skip links that are already marked as a link to be updated + if (_linkFrameData.find(childLink) != _linkFrameData.end()) + continue; + + physics::FrameData3d childLinkFrameData; + if (!this->GetFrameDataRelativeToWorld(childLink, childLinkFrameData)) + continue; + + _linkFrameData[childLink] = childLinkFrameData; + } + + // since nested model poses are saved w.r.t. the nested model's parent + // pose, we must also update any nested models that have a different + // canonical link + for (const auto &nestedModel : model.Models(_ecm)) + { + auto nestedModelCanonicalLinkComp = + _ecm.Component(nestedModel); + if (!nestedModelCanonicalLinkComp) + { + ignerr << "Model [" << nestedModel << "] has no canonical link\n"; + continue; + } + + auto nestedCanonicalLink = nestedModelCanonicalLinkComp->Data(); + + // skip links that are already marked as a link to be updated + if (nestedCanonicalLink == _canonicalLink || + _linkFrameData.find(nestedCanonicalLink) != _linkFrameData.end()) + continue; + + // mark this canonical link as one that needs to be updated so that all of + // the models that have this canonical link are updated + physics::FrameData3d canonicalLinkFrameData; + if (!this->GetFrameDataRelativeToWorld(nestedCanonicalLink, + canonicalLinkFrameData)) + continue; + + _linkFrameData[nestedCanonicalLink] = canonicalLinkFrameData; + } +} + +////////////////////////////////////////////////// +bool PhysicsPrivate::GetFrameDataRelativeToWorld(const Entity _entity, + physics::FrameData3d &_data) +{ + auto entityPhys = this->entityLinkMap.Get(_entity); + if (nullptr == entityPhys) + { + ignerr << "Internal error: entity [" << _entity + << "] not in entity map" << std::endl; + return false; + } + + _data = entityPhys->FrameDataRelativeToWorld(); + return true; +} + ////////////////////////////////////////////////// void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, - const std::map &_linkFrameData) + std::map &_linkFrameData) { IGN_PROFILE("PhysicsPrivate::UpdateSim"); @@ -2100,69 +2252,15 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, // (linkEntity). Since we have the models in topological order and // _linkFrameData stores links in topological order thanks to the ordering // of std::map (entity IDs are created in ascending order), this should - // properly handle pose updates for nested models - for (auto &model : canonicalLinkModels) - { - std::optional parentWorldPose; - - // If this model is nested, the pose of the parent model has already - // been updated since we iterate through the modified links in - // topological order. We expect to find the updated pose in - // this->modelWorldPoses. If not found, this must not be nested, so this - // model's pose component would reflect it's absolute pose. - auto parentModelPoseIt = - this->modelWorldPoses.find( - _ecm.Component(model)->Data()); - if (parentModelPoseIt != this->modelWorldPoses.end()) - { - parentWorldPose = parentModelPoseIt->second; - } - - // Given the following frame names: - // W: World/inertial frame - // P: Parent frame (this could be a parent model or the World frame) - // M: This model's frame - // L: The frame of this model's canonical link - // - // And the following quantities: - // (See http://sdformat.org/tutorials?tut=specify_pose for pose - // convention) - // parentWorldPose (X_WP): Pose of the parent frame w.r.t the world - // linkPoseFromModel (X_ML): Pose of the canonical link frame w.r.t the - // model frame - // linkWorldPose (X_WL): Pose of the canonical link w.r.t the world - // modelWorldPose (X_WM): Pose of this model w.r.t the world - // - // The Pose component of this model entity stores the pose of M w.r.t P - // (X_PM) and is calculated as - // X_PM = (X_WP)^-1 * X_WM - // - // And X_WM is calculated from X_WL, which is obtained from physics as: - // X_WM = X_WL * (X_ML)^-1 - auto linkPoseFromModel = this->RelativePose(model, linkEntity, _ecm); - const auto &linkWorldPose = frameData.pose; - const auto &modelWorldPose = - math::eigen3::convert(linkWorldPose) * linkPoseFromModel.Inverse(); - - this->modelWorldPoses[model] = modelWorldPose; - - // update model's pose - auto modelPose = _ecm.Component(model); - if (parentWorldPose) - { - *modelPose = - components::Pose(parentWorldPose->Inverse() * modelWorldPose); - } - else - { - // This is a non-nested model and parentWorldPose would be identity - // because it would be the pose of the parent (world) w.r.t the world. - *modelPose = components::Pose(modelWorldPose); - } - - _ecm.SetChanged(model, components::Pose::typeId, - ComponentState::PeriodicChange); - } + // properly handle pose updates for nested models that share the same + // canonical link. + // + // Nested models that don't share the same canonical link will also need to + // be updated since these nested models have their pose saved w.r.t. their + // parent model, which just experienced a pose update. The UpdateModelPose + // method also handles this case. + for (auto &modelEnt : canonicalLinkModels) + this->UpdateModelPose(modelEnt, linkEntity, _ecm, _linkFrameData); } IGN_PROFILE_END(); diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index c09e9d74b8..09eef88005 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -564,6 +564,7 @@ TEST_P(DiffDriveTest, OdomCustomFrameId) int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -623,6 +624,7 @@ TEST_P(DiffDriveTest, Pose_VFrameId) int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -682,6 +684,7 @@ TEST_P(DiffDriveTest, Pose_VCustomFrameId) int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index 150f4f3032..7c0c67b9ec 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -261,6 +261,7 @@ class OdometryPublisherTest : public ::testing::TestWithParam int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc index ea6ec02e85..45ac53609c 100644 --- a/test/integration/optical_tactile_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -161,6 +161,7 @@ TEST_F(OpticalTactilePluginTest, // Give some time for messages to propagate sleep = 0; + // cppcheck-suppress knownConditionTrueFalse while (!receivedMsg && sleep < maxSleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index d1e34607a2..5fe9702767 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -1458,3 +1459,132 @@ TEST_F(PhysicsSystemFixture, PhysicsOptions) EXPECT_TRUE(checked); } + +///////////////////////////////////////////////// +// This tests whether pose updates are correct for a model whose canonical link +// changes, but other links do not +TEST_F(PhysicsSystemFixture, MovingCanonicalLinkOnly) +{ + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", + "only_canonical_link_moves.sdf"); + + sdf::Root root; + root.Load(sdfFile); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(nullptr != world); + + serverConfig.SetSdfFile(sdfFile); + + gazebo::Server server(serverConfig); + + server.SetUpdatePeriod(1us); + + // Create a system that records the poses of the links after physics + test::Relay testSystem; + + size_t numBaseLinkChecks = 0; + size_t numOuterLinkChecks = 0; + size_t numBaseLinkCustomChecks = 0; + size_t numOuterLinkCustomChecks = 0; + size_t numBaseLinkParentChecks = 0; + size_t numNestedModelLinkChecks = 0; + size_t numParentModelLinkChecks = 0; + size_t numBaseLinkChildChecks = 0; + + size_t currIter = 0; + testSystem.OnPostUpdate( + [&](const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + { + currIter++; + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Link *, const components::Name *_name)->bool + { + // ignore the link for the ground plane + if (_name->Data() != "surface") + { + const double dt = 0.001; + const double zExpected = + 0.5 * world->Gravity().Z() * pow(dt * currIter, 2); + + if (_name->Data() == "base_link") + { + // link "base_link" falls due to gravity, starting from rest + EXPECT_NEAR(zExpected, + ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2); + numBaseLinkChecks++; + } + else if (_name->Data() == "link0_outer") + { + // link "link0_outer" is resting on the ground and does not + // move, so it should always have a pose of (1 0 0 0 0 0) + EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), + ignition::gazebo::worldPose(_entity, _ecm)); + numOuterLinkChecks++; + } + else if (_name->Data() == "base_link_custom") + { + // same as link "base_link" + EXPECT_NEAR(zExpected, + ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2); + numBaseLinkCustomChecks++; + } + else if (_name->Data() == "link0_outer_custom") + { + // same as "link0_outer", but with an offset pose + EXPECT_EQ(ignition::math::Pose3d(1, 2, 0, 0, 0, 0), + ignition::gazebo::worldPose(_entity, _ecm)); + numOuterLinkCustomChecks++; + } + else if (_name->Data() == "base_link_parent") + { + // same as link "base_link" + EXPECT_NEAR(zExpected, + ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2); + numBaseLinkParentChecks++; + } + else if (_name->Data() == "nested_model_link") + { + // same as "link0_outer", but with an offset pose + EXPECT_EQ(ignition::math::Pose3d(1, -2, 0, 0, 0, 0), + ignition::gazebo::worldPose(_entity, _ecm)); + numNestedModelLinkChecks++; + } + else if (_name->Data() == "parent_model_link") + { + // same as "link0_outer", but with an offset pose + EXPECT_EQ(ignition::math::Pose3d(1, -4, 0, 0, 0, 0), + ignition::gazebo::worldPose(_entity, _ecm)); + numParentModelLinkChecks++; + } + else if (_name->Data() == "base_link_child") + { + // same as link "base_link" + EXPECT_NEAR(zExpected, + ignition::gazebo::worldPose(_entity, _ecm).Z(), 1e-2); + numBaseLinkChildChecks++; + } + } + + return true; + }); + + return true; + }); + + server.AddSystem(testSystem.systemPtr); + const size_t iters = 500; + server.Run(true, iters, false); + + EXPECT_EQ(iters, currIter); + EXPECT_EQ(iters, numBaseLinkChecks); + EXPECT_EQ(iters, numOuterLinkChecks); + EXPECT_EQ(iters, numBaseLinkCustomChecks); + EXPECT_EQ(iters, numOuterLinkCustomChecks); + EXPECT_EQ(iters, numBaseLinkParentChecks); + EXPECT_EQ(iters, numNestedModelLinkChecks); + EXPECT_EQ(iters, numParentModelLinkChecks); + EXPECT_EQ(iters, numBaseLinkChildChecks); +} diff --git a/test/worlds/only_canonical_link_moves.sdf b/test/worlds/only_canonical_link_moves.sdf new file mode 100644 index 0000000000..4afef7d849 --- /dev/null +++ b/test/worlds/only_canonical_link_moves.sdf @@ -0,0 +1,242 @@ + + + + + + + + + 0 0 10 0 0 0 + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 2 2 + + + + + + + + + + + + + 0.2 + 0.1 + + + + 0.0 0.6 0.9 0.4 + 0.0 0.6 0.9 0.4 + + + + + + 1 0 0 0 0 0 + + + + 0.2 + 0.2 + + + + + + + 0.2 + 0.2 + + + + 0.6 0.6 0.9 1 + 0.6 0.6 0.9 1 + 1.0 1.0 1.0 1 + + + + + + + + 0 2 0 0 0 0 + + 1 0 0 0 0 0 + + + + 0.2 + 0.2 + + + + + + + 0.2 + 0.2 + + + + 0.6 0.6 0.9 1 + 0.6 0.6 0.9 1 + 1.0 1.0 1.0 1 + + + + + + + + + 0.2 + 0.1 + + + + 0.0 0.6 0.9 0.4 + 0.0 0.6 0.9 0.4 + + + + + + + + 0 -2 0 0 0 0 + + + + + 0.2 + 0.1 + + + + 0.0 0.6 0.9 0.4 + 0.0 0.6 0.9 0.4 + + + + + + + 1 0 0 0 0 0 + + + + 0.2 + 0.2 + + + + + + + 0.2 + 0.2 + + + + 0.6 0.6 0.9 1 + 0.6 0.6 0.9 1 + 1.0 1.0 1.0 1 + + + + + + + + + 0 -4 0 0 0 0 + + + 1 0 0 0 0 0 + + + + 0.2 + 0.2 + + + + + + + 0.2 + 0.2 + + + + 0.6 0.6 0.9 1 + 0.6 0.6 0.9 1 + 1.0 1.0 1.0 1 + + + + + + + + + 0.2 + 0.1 + + + + 0.0 0.6 0.9 0.4 + 0.0 0.6 0.9 0.4 + + + + + + +