Support FreeGroup features for nested models

dartsim/src/EntityManagementFeatures.cc

@@ -195,7 +195,11 @@ Identity EntityManagementFeatures::GetEngineOfWorld(
 std::size_t EntityManagementFeatures::GetModelCount(
     const Identity &_worldID) const
 {
-  return this->ReferenceInterface<DartWorld>(_worldID)->getNumSkeletons();
+  // dart::simulation::World::getNumSkeletons returns all the skeletons in the
+  // world, including nested ones. We use the size of the indexInContainerToID
+  // vector associated with the _worldID to determine the number of models
+  // that are direct children of the world.
+  return this->models.indexInContainerToID.at(_worldID).size();
 }
 
 /////////////////////////////////////////////////

dartsim/src/FreeGroupFeatures.cc

@@ -19,6 +19,7 @@
 
 #include <dart/constraint/ConstraintSolver.hpp>
 #include <dart/dynamics/FreeJoint.hpp>
+#include <ignition/common/Console.hh>
 
 namespace ignition {
 namespace physics {
@@ -28,13 +29,13 @@ namespace dartsim {
 Identity FreeGroupFeatures::FindFreeGroupForModel(
     const Identity &_modelID) const
 {
+  const auto modelInfo = this->models.at(_modelID);
   // Verify that the model qualifies as a FreeGroup
-  const dart::dynamics::ConstSkeletonPtr &skeleton =
-      this->models.at(_modelID)->model;
+  const dart::dynamics::ConstSkeletonPtr &skeleton = modelInfo->model;
 
   // If there are no bodies at all in this model, then the FreeGroup functions
   // will not work properly, so we'll just reject these cases.
-  if (skeleton->getNumBodyNodes() == 0)
+  if (skeleton->getNumBodyNodes() == 0 && modelInfo->nestedModels.size() == 0)
     return this->GenerateInvalidId();
 
   // Verify that all root joints are FreeJoints
@@ -47,6 +48,26 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
     }
   }
 
+  for (const auto &nestedModel : modelInfo->nestedModels)
+  {
+    // Check that each nested model with BodyNodes or nested models has valid
+    // free groups by recursively calling FindFreeGroupForModel. Nested models
+    // without BodyNodes or their own nested models are disallowed by the
+    // SDFormat spec, yet may occur if all BodyNodes are moved out of a skeleton
+    // when creating joints. In this case, skip such a skeleton instead of
+    // returning an error.
+    auto nestedModelInfo = this->models.at(nestedModel);
+    if (nestedModelInfo->model->getNumBodyNodes() > 0 ||
+        nestedModelInfo->nestedModels.size() > 0)
+    {
+      if (!this->FindFreeGroupForModel(
+              this->GenerateIdentity(nestedModel, nestedModelInfo)))
+      {
+        return this->GenerateInvalidId();
+      }
+    }
+  }
+
   // TODO(MXG): When the dartsim plugin supports closed-loop constraints, verify
   // that this model is not attached to the world or any other models. If it's
   // attached to anything external, then we should return an invalid identity.
@@ -95,9 +116,31 @@ FreeGroupFeatures::FreeGroupInfo FreeGroupFeatures::GetCanonicalInfo(
   const auto model_it = this->models.idToObject.find(_groupID);
   if (model_it != this->models.idToObject.end())
   {
-    return FreeGroupInfo{
-      model_it->second->model->getRootBodyNode(),
-      model_it->second->model.get()};
+    const auto &modelInfo = model_it->second;
+    if (modelInfo->model->getNumBodyNodes() > 0)
+    {
+      return FreeGroupInfo{
+        modelInfo->model->getRootBodyNode(),
+          modelInfo->model.get()};
+    }
+    else
+    {
+      // If the skeleton doesn't have any BodyNodes, we recursively search for
+      // a root BodyNode in the first nested model that has a non-zero number of
+      // BodyNodes. Since all root joints have been verified to be FreeJoints in
+      // FindFreeGroupForModel, we are guaranteed that the BodyNode found in
+      // this way is the root link for the free group.
+      for (const auto &nestedModel : modelInfo->nestedModels)
+      {
+        auto freeGroupInfo =
+          this->GetCanonicalInfo(this->GenerateIdentity(nestedModel));
+        if (freeGroupInfo.link != nullptr)
+          return freeGroupInfo;
+      }
+
+      // Error
+      return {};
+    }
   }
 
   return FreeGroupInfo{this->links.at(_groupID)->link, nullptr};
@@ -111,22 +154,45 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
   const FreeGroupInfo &info = GetCanonicalInfo(_groupID);
   if (!info.model)
   {
-    static_cast<dart::dynamics::FreeJoint*>(info.link->getParentJoint())
+    if (nullptr != info.link)
+    {
+      static_cast<dart::dynamics::FreeJoint*>(info.link->getParentJoint())
         ->setTransform(_pose);
+    }
+    else
+    {
+      ignerr << "No link for free group with id [" << _groupID.id
+             << "] found. SetFreeGroupWorldPose failed." << std::endl;
+    }
     return;
   }
 
-  const Eigen::Isometry3d tf_change =
+  const Eigen::Isometry3d tfChange =
       _pose * info.link->getWorldTransform().inverse();
 
   for (std::size_t i = 0; i < info.model->getNumTrees(); ++i)
   {
     auto *bn = info.model->getRootBodyNode(i);
-    const Eigen::Isometry3d new_tf = tf_change * bn->getTransform();
+    const Eigen::Isometry3d new_tf = tfChange * bn->getTransform();
 
     static_cast<dart::dynamics::FreeJoint*>(bn->getParentJoint())
         ->setTransform(new_tf);
   }
+
+  auto modelInfo = this->models.at(_groupID);
+  for (const auto &nestedModel : modelInfo->nestedModels)
+  {
+    auto nestedModelIdentity = this->GenerateIdentity(nestedModel);
+    const FreeGroupInfo &nestedInfo = GetCanonicalInfo(nestedModelIdentity);
+    // If nestedInfo.link is a nullptr, we skip this model because means the
+    // BodyNodes in this skeleton have been moved to another skeleton and their
+    // pose update will be handled there.
+    if (nullptr != nestedInfo.link && nestedInfo.link != info.link)
+    {
+      this->SetFreeGroupWorldPose(nestedModelIdentity,
+          tfChange * nestedInfo.link->getTransform());
+    }
+  }
 }
 
 /////////////////////////////////////////////////

dartsim/src/FreeGroupFeatures_TEST.cc

@@ -0,0 +1,194 @@
+/*
+ * 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 <gtest/gtest.h>
+
+#include <tuple>
+
+#include <ignition/common/Util.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/eigen3/Conversions.hh>
+#include <ignition/plugin/Loader.hh>
+#include <sdf/Root.hh>
+#include <sdf/World.hh>
+
+#include "ignition/physics/FrameSemantics.hh"
+#include "ignition/physics/FreeGroup.hh"
+#include "ignition/physics/GetEntities.hh"
+#include "ignition/physics/RequestEngine.hh"
+#include "ignition/physics/sdf/ConstructModel.hh"
+#include "ignition/physics/sdf/ConstructNestedModel.hh"
+#include "ignition/physics/sdf/ConstructWorld.hh"
+#include "test/Utils.hh"
+
+struct TestFeatureList : ignition::physics::FeatureList<
+    ignition::physics::sdf::ConstructSdfWorld,
+    ignition::physics::sdf::ConstructSdfModel,
+    ignition::physics::sdf::ConstructSdfNestedModel,
+    ignition::physics::GetWorldFromEngine,
+    ignition::physics::GetModelFromWorld,
+    ignition::physics::GetNestedModelFromModel,
+    ignition::physics::GetLinkFromModel,
+    ignition::physics::LinkFrameSemantics,
+    ignition::physics::FindFreeGroupFeature,
+    ignition::physics::SetFreeGroupWorldPose > { };
+
+using World = ignition::physics::World3d<TestFeatureList>;
+using WorldPtr = ignition::physics::World3dPtr<TestFeatureList>;
+using ModelPtr = ignition::physics::Model3dPtr<TestFeatureList>;
+using LinkPtr = ignition::physics::Link3dPtr<TestFeatureList>;
+
+/////////////////////////////////////////////////
+auto LoadEngine()
+{
+  ignition::plugin::Loader loader;
+  loader.LoadLib(dartsim_plugin_LIB);
+
+  ignition::plugin::PluginPtr dartsim =
+      loader.Instantiate("ignition::physics::dartsim::Plugin");
+
+  auto engine =
+      ignition::physics::RequestEngine3d<TestFeatureList>::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<std::string> names =
+      ignition::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);
+
+  ignition::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(
+        ignition::math::eigen3::convert(parentModelNewPose));
+
+    auto link1 = parentModel->GetLink("link1");
+    ASSERT_NE(nullptr, link1);
+    auto frameData = link1->FrameDataRelativeToWorld();
+    EXPECT_EQ(parentModelNewPose,
+              ignition::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
+    ignition::math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0);
+    ignition::math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, IGN_PI_2);
+
+    ignition::math::Pose3d nestedLinkExpectedPose =
+        parentModelNewPose * nestedModelPose * nestedLinkPose;
+
+    EXPECT_EQ(nestedLinkExpectedPose,
+        ignition::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(
+        ignition::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,
+              ignition::math::eigen3::convert(frameData.pose));
+  }
+}

dartsim/src/SDFFeatures_TEST.cc

@@ -540,7 +540,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel)
   WorldPtr world =
       this->LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf");
   ASSERT_NE(nullptr, world);
-  EXPECT_EQ(4u, world->GetModelCount());
+  EXPECT_EQ(2u, world->GetModelCount());
 
   dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld();
   ASSERT_NE(nullptr, dartWorld);
@@ -599,7 +599,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld)
   WorldPtr world = this->LoadWorld(
       TEST_WORLD_DIR "/world_with_nested_model_joint_to_world.sdf");
   ASSERT_NE(nullptr, world);
-  EXPECT_EQ(2u, world->GetModelCount());
+  EXPECT_EQ(1u, world->GetModelCount());
 
   dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld();
   ASSERT_NE(nullptr, dartWorld);

dartsim/worlds/world_with_nested_model.sdf

@@ -63,5 +63,13 @@
         <link name="link1"/>
       </model>
     </model>
+
+    <model name="parent_model2">
+      <model name="child_model">
+        <model name="grand_child_model">
+          <link name="link1"/>
+        </model>
+      </model>
+    </model>
   </world>
 </sdf>

tpe/lib/src/Model.cc

@@ -70,7 +70,7 @@ Entity &Model::AddLink()
 {
   std::size_t linkId = Entity::GetNextId();
 
-  if (this->GetChildren().empty())
+  if (this->GetLinkCount() == 0)
   {
     this->dataPtr->firstLinkId = linkId;
     this->dataPtr->canonicalLinkId = linkId;