update bullet ver check in test, simplify joint to link pose function…

…, fix model transform

Signed-off-by: Ian Chen <[email protected]>

bullet-featherstone/src/FreeGroupFeatures.cc

@@ -33,11 +33,6 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
   if (model->body->hasFixedBase())
     return this->GenerateInvalidId();
 
-  // bullet-featherstone does not allow floating bodies so if a joint exists
-  // the multibody does not quality as a FreeGroup
-  if (model->body->getNumDofs() > 0)
-    return this->GenerateInvalidId();
-
   return _modelID;
 }
 
@@ -50,11 +45,6 @@ Identity FreeGroupFeatures::FindFreeGroupForLink(
   if (model->body->hasFixedBase())
     return this->GenerateInvalidId();
 
-  // bullet-featherstone does not allow floating bodies so if a joint exists
-  // the multibody does not quality as a FreeGroup
-  if (model->body->getNumDofs() > 0)
-    return this->GenerateInvalidId();
-
   return link->model;
 }
 

bullet-featherstone/src/SDFFeatures.cc

@@ -145,57 +145,6 @@ void extractInertial(
   _linkToPrincipalAxesPose.Rot() *= M.PrincipalAxesOffset();
 }
 
-/////////////////////////////////////////////////
-/// \brief Get the pose of a link relative to the input model
-/// The input link can be a nested link.
-/// \param[out] _pose Pose of link relative to model
-/// \param[in] _linkName Scoped name of link
-/// \param[in] _model Model SDF
-::sdf::Errors linkPoseInModelTree(math::Pose3d &_pose,
-    const std::string &_linkName,
-    const ::sdf::Model *_model)
-{
-  ::sdf::Errors errors;
-  size_t idx = _linkName.find("::");
-  if (idx != std::string::npos)
-  {
-    if (_model->ModelCount() > 0)
-    {
-      std::string nestedModelName = _linkName.substr(0, idx);
-      std::string nestedLinkName = _linkName.substr(idx + 2);
-      const auto *nestedModel = _model->ModelByName(nestedModelName);
-      if (!nestedModel)
-      {
-        gzerr << "Unable to find nested model " << nestedModelName << std::endl;
-        return errors;
-      }
-
-      math::Pose3d p;
-      errors = nestedModel->SemanticPose().Resolve(p);
-      if (!errors.empty())
-        return errors;
-      _pose = _pose * p;
-      return linkPoseInModelTree(_pose, nestedLinkName, nestedModel);
-    }
-  }
-  else
-  {
-    const auto *link = _model->LinkByName(_linkName);
-    if (!link)
-    {
-      gzerr << "Unable to find link " << _linkName << std::endl;
-      return errors;
-    }
-    math::Pose3d p;
-    errors = link->SemanticPose().Resolve(p);
-    if (!errors.empty())
-      return errors;
-
-    _pose = _pose * p;
-  }
-  return errors;
-}
-
 /////////////////////////////////////////////////
 /// \brief Get pose of joint relative to link
 /// \param[out] _resolvedPose Pose of joint relative to link
@@ -215,26 +164,22 @@ ::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose,
     return errors;
   }
 
-  math::Pose3d jointPose;
-  errors = joint->SemanticPose().Resolve(jointPose);
-  if (!errors.empty())
-    return errors;
-
-  // joint pose is expressed relative to child link
   std::string childLinkName;
   errors = joint->ResolveChildLink(childLinkName);
   if (!errors.empty())
   {
     childLinkName = joint->ChildName();
   }
 
+  // joint pose is expressed relative to child link so return joint pose
+  // if input link is the child link
   if (childLinkName == _link)
   {
-    _resolvedPose = jointPose;
+    errors = joint->SemanticPose().Resolve(_resolvedPose);
     return errors;
   }
 
-  // pose of parent link
+  // compute joint pose relative to parent link
   const auto *link = _model->LinkByName(_link);
   if (!link)
   {
@@ -243,20 +188,16 @@ ::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose,
    return errors;
   }
 
-  math::Pose3d parentLinkPose;
-  errors = linkPoseInModelTree(parentLinkPose, _link, _model);
-  if (!errors.empty())
-    return errors;
-
-  // pose of child link
-  math::Pose3d childLinkPose;
-  errors = linkPoseInModelTree(childLinkPose, childLinkName, _model);
-  if (!errors.empty())
-    return errors;
+  math::Pose3d jointPoseRelToModel;
+  errors = _model->SemanticPose().Resolve(jointPoseRelToModel,
+      _model->Name() + "::" + _joint);
+  jointPoseRelToModel = jointPoseRelToModel.Inverse();
 
-  auto jointPoseInModel = childLinkPose * jointPose;
+  math::Pose3d modelPoseRelToLink;
+  errors = _model->SemanticPose().Resolve(modelPoseRelToLink,
+      _model->Name() + "::" + _link);
 
-  _resolvedPose = parentLinkPose.Inverse() * jointPoseInModel;
+  _resolvedPose = modelPoseRelToLink * jointPoseRelToModel;
 
   return errors;
 }
@@ -856,20 +797,60 @@ Identity SDFFeatures::ConstructSdfModelImpl(
   model->body->setHasSelfCollision(_sdfModel.SelfCollide());
   model->body->finalizeMultiDof();
 
-  // Note: this assumes the root link is in the top level model
-  // \todo(iche033) consider handling the case when the root link is
-  // in a nested model
-  const auto worldToModel = ResolveSdfPose(structure.model->SemanticPose());
+  const auto worldToModel = ResolveSdfPose(_sdfModel.SemanticPose());
   if (!worldToModel)
     return this->GenerateInvalidId();
 
+  auto modelToNestedModel = Eigen::Isometry3d::Identity();
+  // if the model of the root link is in a top level model, compute
+  // its pose relative to top level model
+  if (structure.model != &_sdfModel)
+  {
+    // get the scoped name of the model
+    // This is used to resolve the model pose
+    std::string modelScopedName;
+    auto getModelScopedName = [&](const ::sdf::Model *_targetModel,
+      const ::sdf::Model *_parentModel, const std::string &_prefix,
+      auto &&_getModelScopedName)
+    {
+      for (std::size_t i = 0u; i < _parentModel->ModelCount(); ++i)
+      {
+        auto childModel =_parentModel->ModelByIndex(i);
+        if (childModel == _targetModel)
+        {
+          modelScopedName = _prefix + childModel->Name();
+          return true;
+        }
+        else
+        {
+          if (_getModelScopedName(_targetModel, childModel,
+              _prefix + childModel->Name() + "::", _getModelScopedName))
+            return true;
+        }
+      }
+      return false;
+    };
+
+    math::Pose3d modelPose;
+    if (!getModelScopedName(structure.model, &_sdfModel,
+        _sdfModel.Name() + "::", getModelScopedName))
+      return this->GenerateInvalidId();
+
+    auto errors = _sdfModel.SemanticPose().Resolve(modelPose,
+        modelScopedName);
+    if (!errors.empty())
+      return this->GenerateInvalidId();
+    modelToNestedModel = math::eigen3::convert(modelPose).inverse();
+  }
+
   const auto modelToRootLink =
     ResolveSdfPose(structure.rootLink->SemanticPose());
   if (!modelToRootLink)
     return this->GenerateInvalidId();
 
   const auto worldToRootCom =
-    *worldToModel * *modelToRootLink * rootInertialToLink.inverse();
+    *worldToModel * modelToNestedModel * *modelToRootLink *
+    rootInertialToLink.inverse();
 
   model->body->setBaseWorldTransform(convertTf(worldToRootCom));
   model->body->setBaseVel(btVector3(0, 0, 0));

test/common_test/CMakeLists.txt

@@ -1,5 +1,4 @@
 set(TEST_TYPE "COMMON_TEST")
-include_directories(${BULLET_INCLUDE_DIRS})
 
 set(tests
   added_mass
@@ -47,6 +46,13 @@ endfunction()
 
 set(GZ_PHYSICS_RESOURCE_DIR "${CMAKE_SOURCE_DIR}/resources")
 
+# Get bullet version using pkg_check_modules as it is not available
+# through the cmake module
+gz_pkg_check_modules_quiet(bullet_version_check "bullet")
+if (bullet_version_check_FOUND)
+  string(REPLACE "." ""  GzBullet_VERSION ${bullet_version_check_VERSION})
+endif()
+
 foreach(test ${tests})
   set(test_executable "${TEST_TYPE}_${test}")
   add_executable(${test_executable} ${test}.cc)
@@ -65,6 +71,7 @@ foreach(test ${tests})
   target_compile_definitions(${test_executable} PRIVATE
     "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\""
     "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\""
+    "BT_BULLET_VERSION=${GzBullet_VERSION}"
   )
 
   install(TARGETS ${test_executable} DESTINATION ${TEST_INSTALL_DIR})

test/common_test/joint_transmitted_wrench_features.cc

@@ -17,7 +17,6 @@
 #include <gtest/gtest.h>
 
 #include <chrono>
-#include <LinearMath/btScalar.h>
 
 #include <gz/common/Console.hh>
 #include <gz/plugin/Loader.hh>
@@ -225,12 +224,10 @@ TYPED_TEST(JointTransmittedWrenchFixture, PendulumAtZeroAngle)
 TYPED_TEST(JointTransmittedWrenchFixture, PendulumInMotion)
 {
   // This test requires https://github.com/bulletphysics/bullet3/pull/4462
-  // When removing this check, also remove
-  // `#include <LinearMath/btScalar.h>` at the top of this file, and
-  // `include_directories(${BULLET_INCLUDE_DIRS})` from
-  // test/common_test/CMakeLists.txt
-  if (this->engineName == "bullet-featherstone" && btGetVersion() <= 325)
+#if BT_BULLET_VERSION <= 325
+  if (this->engineName == "bullet-featherstone")
     GTEST_SKIP();
+#endif
 
   // Start pendulum at 90° (parallel to the ground) and stop at about 40°
   // so that we have non-trivial test expectations.
@@ -418,12 +415,10 @@ TYPED_TEST(JointTransmittedWrenchFixture, JointLosses)
 TYPED_TEST(JointTransmittedWrenchFixture, ContactForces)
 {
   // This test requires https://github.com/bulletphysics/bullet3/pull/4462
-  // When removing this check, also remove
-  // `#include <LinearMath/btScalar.h>` at the top of this file, and
-  // `include_directories(${BULLET_INCLUDE_DIRS})` from
-  // test/common_test/CMakeLists.txt
-  if (this->engineName == "bullet-featherstone" && btGetVersion() <= 325)
+#if BT_BULLET_VERSION <= 325
+  if (this->engineName == "bullet-featherstone")
     GTEST_SKIP();
+#endif
 
   auto box = this->world->GetModel("box");
   ASSERT_NE(nullptr, box);

test/common_test/world_features.cc

@@ -16,8 +16,6 @@
 */
 #include <gtest/gtest.h>
 
-#include <LinearMath/btScalar.h>
-
 #include <gz/common/Console.hh>
 #include <gz/plugin/Loader.hh>
 
@@ -419,12 +417,13 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel)
     joint.SetParentName("world");
     joint.SetChildName("invalid_link");
     EXPECT_FALSE(worldModel->ConstructJoint(joint));
-    if (PhysicsEngineName(name) != "bullet-featherstone" ||
-        btGetVersion() >= 289)
-    {
-      joint.SetChildName("parent_model::link1");
+    joint.SetChildName("parent_model::link1");
+    if (PhysicsEngineName(name) != "bullet-featherstone")
       EXPECT_TRUE(worldModel->ConstructJoint(joint));
-    }
+    else
+#if BT_BULLET_VERSION >= 289
+      EXPECT_TRUE(worldModel->ConstructJoint(joint));
+#endif
 
     gz::physics::ForwardStep::Input input;
     gz::physics::ForwardStep::State state;