Merge branch 'main' into adlarkin/write_step_output_data

.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@master
+        uses: ignition-tooling/action-ignition-ci@bionic
         with:
           codecov-token: ${{ secrets.CODECOV_TOKEN }}
   focal-ci:

README.md

@@ -9,9 +9,9 @@
 
 Build | Status
 -- | --
-Test coverage | [![codecov](https://codecov.io/gh/ignitionrobotics/ign-physics/branch/master/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/ign-physics)
-Ubuntu Bionic | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-master-bionic-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-master-bionic-amd64)
-Homebrew      | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-master-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-master-homebrew-amd64)
+Test coverage | [![codecov](https://codecov.io/gh/ignitionrobotics/ign-physics/branch/main/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/ign-physics)
+Ubuntu Bionic | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-main-bionic-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-main-bionic-amd64)
+Homebrew      | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-main-homebrew-amd64)
 Windows       | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ign_physics-ci-win)](https://build.osrfoundation.org/job/ign_physics-ci-win)
 
 Ignition Physics, a component of [Ignition
@@ -72,7 +72,7 @@ See the [installation tutorial](https://ignitionrobotics.org/api/physics/3.0/ins
 
 # Usage
 
-Please refer to the [examples directory](https://github.com/ignitionrobotics/ign-physics/raw/master/examples/).
+Please refer to the [examples directory](https://github.com/ignitionrobotics/ign-physics/raw/main/examples/).
 
 # Documentation
 
@@ -89,7 +89,7 @@ You can also generate the documentation from a clone of this repository by follo
 2. Clone the repository
 
     ```
-    git clone https://github.com/ignitionrobotics/ign-physics -b ign-physics3
+    git clone https://github.com/ignitionrobotics/ign-physics -b main
     ```
 
 3. Configure and build the documentation.
@@ -147,18 +147,17 @@ ign-physics
 ```
 # Contributing
 
-Please see
-[CONTRIBUTING.md](https://github.com/ignitionrobotics/ign-gazebo/blob/master/CONTRIBUTING.md).
+Please see the [contribution guide](https://ignitionrobotics.org/docs/all/contributing).
 
 # Code of Conduct
 
 Please see
-[CODE\_OF\_CONDUCT.md](https://github.com/ignitionrobotics/ign-gazebo/blob/master/CODE_OF_CONDUCT.md).
+[CODE\_OF\_CONDUCT.md](https://github.com/ignitionrobotics/ign-gazebo/blob/main/CODE_OF_CONDUCT.md).
 
 # Versioning
 
 This library uses [Semantic Versioning](https://semver.org/). Additionally, this library is part of the [Ignition Robotics project](https://ignitionrobotics.org) which periodically releases a versioned set of compatible and complimentary libraries. See the [Ignition Robotics website](https://ignitionrobotics.org) for version and release information.
 
 # License
 
-This library is licensed under [Apache 2.0](https://www.apache.org/licenses/LICENSE-2.0). See also the [LICENSE](https://github.com/ignitionrobotics/ign-physics/blob/master/LICENSE) file.
+This library is licensed under [Apache 2.0](https://www.apache.org/licenses/LICENSE-2.0). See also the [LICENSE](https://github.com/ignitionrobotics/ign-physics/blob/main/LICENSE) file.

dartsim/src/Base.hh

@@ -288,7 +288,8 @@ class Base : public Implements3d<FeatureList<Feature>>
     return std::forward_as_tuple(id, entry);
   }
 
-  public: inline std::size_t AddLink(DartBodyNode *_bn)
+  public: inline std::size_t AddLink(DartBodyNode *_bn,
+                                     const std::string &_fullName)
   {
     const std::size_t id = this->GetNextEntity();
     this->links.idToObject[id] = std::make_shared<LinkInfo>();
@@ -299,6 +300,8 @@ class Base : public Implements3d<FeatureList<Feature>>
     this->links.objectToID[_bn] = id;
     this->frames[id] = _bn;
 
+    this->linksByName[_fullName] = _bn;
+
     return id;
   }
 
@@ -326,6 +329,7 @@ class Base : public Implements3d<FeatureList<Feature>>
   public: void RemoveModelImpl(const std::size_t _worldID,
                                const std::size_t _modelID)
   {
+    // TODO(addisu) Handle removal of nested models
     const auto &world = this->worlds.at(_worldID);
     auto skel = this->models.at(_modelID)->model;
     // Remove the contents of the skeleton from local entity storage containers
@@ -340,76 +344,24 @@ class Base : public Implements3d<FeatureList<Feature>>
         this->shapes.RemoveEntity(sn);
       }
       this->links.RemoveEntity(bn);
+      this->linksByName.erase(::sdf::JoinName(
+          world->getName(), ::sdf::JoinName(skel->getName(), bn->getName())));
     }
     this->models.RemoveEntity(skel);
     world->removeSkeleton(skel);
   }
 
-  static bool SkeletonNameEndsWithModelName(
-      const std::string &_skeletonName, const std::string &_modelName)
-  {
-    if (_modelName.empty())
-      return true;
-
-    if (_skeletonName.size() < _modelName.size())
-      return false;
-
-    // If _modelName comes from a nested model, it might actually be shorter
-    // than _skeletonName. E.g -> _skeletonName = a::b::c, _modelName = b::c
-    const size_t compareStartPosition =
-      _skeletonName.size() - _modelName.size();
-    return _skeletonName.compare(
-      compareStartPosition, _modelName.size(), _modelName) == 0;
-  }
-
-  /// \brief Finds the skeleton in the world that matches the qualified name
-  /// If more than one match exists, it will return null
-  /// \param[in] _worldID Which world to search.
-  /// \param[in] _name Name of entity.
-  /// \return Pointer to skeleton if exactly one match was found, otherwise null
-  public: DartSkeletonPtr FindContainingSkeletonFromName(
-      const dart::simulation::WorldPtr &_world, const std::string &_name)
-  {
-    if (_world == nullptr)
-      return nullptr;
-
-    if (_name == "world")
-      return nullptr;
-
-    const auto[skeletonName, entityName] = ::sdf::SplitName(_name);
-    std::vector<DartSkeletonPtr> matches;
-    for (size_t i = 0; i < _world->getNumSkeletons(); ++i)
-    {
-      auto candidateSkeleton = _world->getSkeleton(i);
-      if (SkeletonNameEndsWithModelName(
-        candidateSkeleton->getName(), skeletonName))
-      {
-        // There may be multiple skeletons that match the model name, only match
-        // those that contain a matching entity
-        auto * node = candidateSkeleton->getBodyNode(entityName);
-        if (nullptr != node)
-          matches.push_back(candidateSkeleton);
-      }
-    }
-
-    // It's possible there was more than 1 match
-    // (e.g. b::c matches both a::b::c and d::b::c)
-    if (matches.size() == 1)
-      return matches.front();
-    else if (matches.size() > 1)
-    {
-      ignerr << "Found " << matches.size() << " for " << _name << std::endl;
-      return nullptr;
-    }
-    return nullptr;
-  }
-
   public: EntityStorage<DartWorldPtr, std::string> worlds;
   public: EntityStorage<ModelInfoPtr, DartConstSkeletonPtr> models;
   public: EntityStorage<LinkInfoPtr, const DartBodyNode*> links;
   public: EntityStorage<JointInfoPtr, const DartJoint*> joints;
   public: EntityStorage<ShapeInfoPtr, const DartShapeNode*> shapes;
   public: std::unordered_map<std::size_t, dart::dynamics::Frame*> frames;
+
+  /// \brief Map from the fully qualified link name (including the world name)
+  /// to the BodyNode object. This is useful for keeping track of BodyNodes even
+  /// as they move to other skeletons.
+  public: std::unordered_map<std::string, DartBodyNode*> linksByName;
 };
 
 }

dartsim/src/Base_TEST.cc

@@ -65,7 +65,10 @@ TEST(BaseClass, RemoveModel)
       boxShape);
 
     auto res = base.AddModel({skel, frame, ""}, worldID);
-    base.AddLink(pair.second);
+    const std::string fullName = ::sdf::JoinName(
+        world->getName(),
+        ::sdf::JoinName(skel->getName(), pair.second->getName()));
+    base.AddLink(pair.second, fullName);
     base.AddJoint(pair.first);
     base.AddShape({sn, name + "_shape"});
 
@@ -80,6 +83,7 @@ TEST(BaseClass, RemoveModel)
 
   EXPECT_EQ(5u, base.models.size());
   EXPECT_EQ(5u, base.links.size());
+  EXPECT_EQ(5u, base.linksByName.size());
   EXPECT_EQ(5u, base.joints.size());
   EXPECT_EQ(5u, base.shapes.size());
 
@@ -93,6 +97,7 @@ TEST(BaseClass, RemoveModel)
   // Check that other resouces (links, shapes, etc) are also removed
   EXPECT_EQ(4u, base.models.size());
   EXPECT_EQ(4u, base.links.size());
+  EXPECT_EQ(4u, base.linksByName.size());
   EXPECT_EQ(4u, base.joints.size());
   EXPECT_EQ(4u, base.shapes.size());
 
@@ -126,6 +131,7 @@ TEST(BaseClass, RemoveModel)
     --curSize;
     EXPECT_EQ(curSize, base.models.size());
     EXPECT_EQ(curSize, base.links.size());
+    EXPECT_EQ(curSize, base.linksByName.size());
     EXPECT_EQ(curSize, base.joints.size());
     EXPECT_EQ(curSize, base.shapes.size());
     checkModelIndices();

dartsim/src/EntityManagementFeatures.cc

@@ -653,7 +653,21 @@ Identity EntityManagementFeatures::ConstructEmptyLink(
       model->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(
         nullptr, prop_fj, prop_bn).second;
 
-  const std::size_t linkID = this->AddLink(bn);
+  auto worldIDIt = this->models.idToContainerID.find(_modelID);
+  if (worldIDIt == this->models.idToContainerID.end())
+  {
+    ignerr << "World of model [" << model->getName()
+           << "] could not be found when creating link [" << _name
+           << "]\n";
+    return this->GenerateInvalidId();
+  }
+
+  auto world = this->worlds.at(worldIDIt->second);
+  const std::string fullName = ::sdf::JoinName(
+      world->getName(),
+      ::sdf::JoinName(model->getName(), bn->getName()));
+
+  const std::size_t linkID = this->AddLink(bn, fullName);
   return this->GenerateIdentity(linkID, this->links.at(linkID));
 }
 

dartsim/src/SDFFeatures.cc

@@ -330,31 +330,22 @@ static ShapeAndTransform ConstructGeometry(
 }  // namespace
 
 dart::dynamics::BodyNode *SDFFeatures::FindBodyNode(
-    dart::simulation::WorldPtr _world, const std::string _jointModelName,
-    const std::string &_entityFullName)
+    const std::string &_worldName, const std::string &_jointModelName,
+    const std::string &_linkRelativeName)
 {
-  const auto[modelAbsoluteName, localName] =
-      ::sdf::SplitName(_entityFullName);
-
-  if (localName == "world")
-    return nullptr;
-
-  const auto modelSkeleton = _world->getSkeleton(_jointModelName);
-
-  // First check the obvious place
-  if (modelSkeleton != nullptr
-      && modelSkeleton->getBodyNode(localName) != nullptr)
-    return modelSkeleton->getBodyNode(localName);
-
-  // For nested situations where the entity's name doesn't contain the fully-
-  // qualified skeleton name (e.g. skeleton = "a::b::c", but the entity name
-  // contains "b::c")
-  auto matchedSkeleton =
-    this->FindContainingSkeletonFromName(_world, _entityFullName);
-  if (matchedSkeleton == nullptr)
+  if (_linkRelativeName == "world")
     return nullptr;
 
-  return matchedSkeleton->getBodyNode(localName);
+  const auto fullName = ::sdf::JoinName(
+      _worldName, ::sdf::JoinName(_jointModelName, _linkRelativeName));
+  auto it = this->linksByName.find(fullName);
+  if (it != this->linksByName.end())
+  {
+    return it->second;
+  }
+  ignerr << "Could not find link " << _linkRelativeName << " in model "
+         << _jointModelName << std::endl;
+  return nullptr;
 }
 
 /////////////////////////////////////////////////
@@ -470,7 +461,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
              << "] of joint [" << sdfJoint->Name() << "] in model ["
              << modelName
              << "] could not be resolved. The joint will not be constructed\n";
-      for (const auto error : errors)
+      for (const auto &error : errors)
       {
         ignerr << error << std::endl;
       }
@@ -484,7 +475,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
              << "] of joint [" << sdfJoint->Name() << "] in model ["
              << modelName
              << "] could not be resolved. The joint will not be constructed\n";
-      for (const auto error : errors)
+      for (const auto &error : errors)
       {
         ignerr << error << std::endl;
       }
@@ -512,7 +503,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
     }
 
     auto * const parent =
-      FindBodyNode(this->worlds[worldID], modelName, parentLinkName);
+      FindBodyNode(this->worlds[worldID]->getName(), modelName, parentLinkName);
 
     if (nullptr == parent && parentLinkName != "world")
     {
@@ -523,7 +514,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
     }
 
     auto * const child =
-      FindBodyNode(worlds[worldID], modelName, childSdfLink->Name());
+      FindBodyNode(worlds[worldID]->getName(), modelName, childLinkName);
     if (nullptr == child)
     {
       ignerr << "The child of joint [" << sdfJoint->Name() << "] in model ["
@@ -586,7 +577,20 @@ Identity SDFFeatures::ConstructSdfLink(
 
   dart::dynamics::BodyNode * const bn = result.second;
 
-  const std::size_t linkID = this->AddLink(bn);
+  auto worldIDIt = this->models.idToContainerID.find(_modelID);
+  if (worldIDIt == this->models.idToContainerID.end())
+  {
+    ignerr << "World of model [" << modelInfo.model->getName()
+           << "] could not be found when creating link [" << _sdfLink.Name()
+           << "]\n";
+    return this->GenerateInvalidId();
+  }
+
+  auto world = this->worlds.at(worldIDIt->second);
+  const std::string fullName = ::sdf::JoinName(
+      world->getName(),
+      ::sdf::JoinName(modelInfo.model->getName(), bn->getName()));
+  const std::size_t linkID = this->AddLink(bn, fullName);
   this->AddJoint(joint);
 
   auto linkIdentity = this->GenerateIdentity(linkID, this->links.at(linkID));
@@ -630,6 +634,15 @@ Identity SDFFeatures::ConstructSdfJoint(
 {
   const auto &modelInfo = *this->ReferenceInterface<ModelInfo>(_modelID);
 
+  if (_sdfJoint.ChildLinkName() == "world")
+  {
+    ignerr << "Asked to create a joint with the world as the child in model "
+           << "[" << modelInfo.model->getName() << "]. This is currently not "
+           << "supported\n";
+
+    return this->GenerateInvalidId();
+  }
+
   // If we get an error here, one of two things has occured:
   // 1. The Model _sdfJoint came from is invalid, i.e, had errors during
   //    sdf::Root::Load and ConstructSdfJoint was called regardless of the
@@ -663,7 +676,7 @@ Identity SDFFeatures::ConstructSdfJoint(
   }
 
   dart::dynamics::BodyNode * const parent =
-    FindBodyNode(world, modelInfo.model->getName(), parentLinkName);
+    FindBodyNode(world->getName(), modelInfo.model->getName(), parentLinkName);
 
   std::string childLinkName;
   const auto childResolveErrors = _sdfJoint.ResolveChildLink(childLinkName);
@@ -672,7 +685,7 @@ Identity SDFFeatures::ConstructSdfJoint(
   }
 
   dart::dynamics::BodyNode * const child =
-    FindBodyNode(world, modelInfo.model->getName(), childLinkName);
+    FindBodyNode(world->getName(), modelInfo.model->getName(), childLinkName);
 
   if (nullptr == parent && parentLinkName != "world")
   {

dartsim/src/SDFFeatures.hh

@@ -112,16 +112,18 @@ class SDFFeatures :
   private: Identity ConstructSdfModelImpl(std::size_t _parentID,
                                           const ::sdf::Model &_sdfModel);
 
-  /// \brief Find the dartsim BodyNode associated with the entity name
-  /// \param[in] _world Pointer to world to search through
+  /// \brief Find the dartsim BodyNode associated with the link name
+  /// \param[in] _worldName Name of world that contains the link
   /// \param[in] _jointModelName The name of the model associated with the joint
-  /// \param[in] _entityFullName The full name of the entity as specified in the
-  /// sdformat description
+  /// \param[in] _linkRelativeName The relative name of the link as specified in
+  /// the sdformat description in the scope of the model identified by
+  /// _jointModelName
   /// \returns The matched body node if exactly one match is found, otherwise
   /// a nullptr
   private: dart::dynamics::BodyNode *FindBodyNode(
-      dart::simulation::WorldPtr _world, const std::string _jointModelName,
-      const std::string &_entityFullName);
+               const std::string &_worldName,
+               const std::string &_jointModelName,
+               const std::string &_linkRelativeName);
 };
 
 }