Merge branch 'gz-physics7' into azeey/rel_paths

bullet-featherstone/src/JointFeatures.cc

@@ -368,8 +368,11 @@ void JointFeatures::SetJointTransformFromParent(
 
   if (jointInfo->fixedConstraint)
   {
+      auto tf = convertTf(_pose);
       jointInfo->fixedConstraint->setPivotInA(
-        convertVec(_pose.translation()));
+        tf.getOrigin());
+      jointInfo->fixedConstraint->setFrameInA(
+        tf.getBasis());
   }
 }
 

bullet-featherstone/src/SDFFeatures.cc

@@ -1205,7 +1205,24 @@ bool SDFFeatures::AddSdfCollision(
 
       auto *world = this->ReferenceInterface<WorldInfo>(model->world);
 
-      if (isStatic)
+      // Set static filter for collisions in
+      // 1) a static model
+      // 2) a fixed base link
+      // 3) a (non-base) link with zero dofs
+      bool isFixed = false;
+      if (model->body->hasFixedBase())
+      {
+        // check if it's a base link
+        isFixed = std::size_t(_linkID) ==
+            static_cast<std::size_t>(model->body->getUserIndex());
+        // check if link has zero dofs
+        if (!isFixed && linkInfo->indexInModel.has_value())
+        {
+           isFixed = model->body->getLink(
+              linkInfo->indexInModel.value()).m_dofCount == 0;
+        }
+      }
+      if (isStatic || isFixed)
       {
         world->world->addCollisionObject(
           linkInfo->collider.get(),

test/common_test/collisions.cc

@@ -16,28 +16,34 @@
 */
 #include <gtest/gtest.h>
 
+#include <sstream>
 #include <string>
 
 #include <gz/common/Console.hh>
 #include <gz/plugin/Loader.hh>
 
 #include "test/Resources.hh"
 #include "test/TestLibLoader.hh"
+#include "Worlds.hh"
 
 #include <gz/physics/FindFeatures.hh>
 #include <gz/physics/RequestEngine.hh>
 #include <gz/physics/ConstructEmpty.hh>
 #include <gz/physics/ForwardStep.hh>
 #include <gz/physics/FrameSemantics.hh>
 #include <gz/physics/FreeJoint.hh>
+#include <gz/physics/GetContacts.hh>
 #include <gz/physics/GetEntities.hh>
 #include <gz/physics/mesh/MeshShape.hh>
 #include <gz/physics/PlaneShape.hh>
 #include <gz/physics/FixedJoint.hh>
+#include <gz/physics/sdf/ConstructModel.hh>
 #include <gz/physics/sdf/ConstructWorld.hh>
 
 #include <gz/common/MeshManager.hh>
 
+#include <sdf/Root.hh>
+
 template <class T>
 class CollisionTest:
   public testing::Test, public gz::physics::TestLibLoader
@@ -141,6 +147,125 @@ TYPED_TEST(CollisionTest, MeshAndPlane)
   }
 }
 
+using CollisionStaticFeaturesList = gz::physics::FeatureList<
+  gz::physics::sdf::ConstructSdfModel,
+  gz::physics::sdf::ConstructSdfWorld,
+  gz::physics::GetContactsFromLastStepFeature,
+  gz::physics::ForwardStep
+>;
+
+using CollisionStaticTestFeaturesList =
+  CollisionTest<CollisionStaticFeaturesList>;
+
+TEST_F(CollisionStaticTestFeaturesList, StaticCollisions)
+{
+  auto getBoxStaticStr = [](const std::string &_name,
+                            const gz::math::Pose3d &_pose)
+  {
+    std::stringstream modelStaticStr;
+    modelStaticStr << R"(
+    <sdf version="1.11">
+      <model name=")";
+    modelStaticStr << _name << R"(">
+        <pose>)";
+    modelStaticStr << _pose;
+    modelStaticStr << R"(</pose>
+        <link name="body">
+          <collision name="collision">
+            <geometry>
+              <box><size>1 1 1</size></box>
+            </geometry>
+          </collision>
+        </link>
+        <static>true</static>
+      </model>
+    </sdf>)";
+    return modelStaticStr.str();
+  };
+
+  auto getBoxFixedJointStr = [](const std::string &_name,
+                                const gz::math::Pose3d &_pose)
+  {
+    std::stringstream modelFixedJointStr;
+    modelFixedJointStr << R"(
+    <sdf version="1.11">
+      <model name=")";
+    modelFixedJointStr << _name << R"(">
+        <pose>)";
+    modelFixedJointStr << _pose;
+    modelFixedJointStr << R"(</pose>
+        <link name="body">
+          <collision name="collision">
+            <geometry>
+              <box><size>1 1 1</size></box>
+            </geometry>
+          </collision>
+        </link>
+        <joint name="world_fixed" type="fixed">
+          <parent>world</parent>
+          <child>body</child>
+        </joint>
+      </model>
+    </sdf>)";
+    return modelFixedJointStr.str();
+  };
+
+  for (const std::string &name : this->pluginNames)
+  {
+    std::cout << "Testing plugin: " << name << std::endl;
+    gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
+
+    sdf::Root rootWorld;
+    const sdf::Errors errorsWorld =
+        rootWorld.Load(common_test::worlds::kGroundSdf);
+    ASSERT_TRUE(errorsWorld.empty()) << errorsWorld.front();
+
+    auto engine =
+        gz::physics::RequestEngine3d<CollisionStaticFeaturesList>::From(plugin);
+    ASSERT_NE(nullptr, engine);
+
+    auto world = engine->ConstructWorld(*rootWorld.WorldByIndex(0));
+    ASSERT_NE(nullptr, world);
+
+    sdf::Root root;
+    sdf::Errors errors = root.LoadSdfString(getBoxStaticStr(
+        "box_static", gz::math::Pose3d::Zero));
+    ASSERT_TRUE(errors.empty()) << errors.front();
+    ASSERT_NE(nullptr, root.Model());
+    world->ConstructModel(*root.Model());
+
+    gz::physics::ForwardStep::Output output;
+    gz::physics::ForwardStep::State state;
+    gz::physics::ForwardStep::Input input;
+    for (std::size_t i = 0; i < 10; ++i)
+    {
+      world->Step(output, state, input);
+    }
+
+    // static box overlaps with ground plane
+    // verify no contacts between static bodies.
+    auto contacts = world->GetContactsFromLastStep();
+    EXPECT_EQ(0u, contacts.size());
+
+    // currently only bullet-featherstone skips collision checking between
+    // static bodies and bodies with world fixed joint
+    if (this->PhysicsEngineName(name) != "bullet-featherstone")
+      continue;
+
+    errors = root.LoadSdfString(getBoxFixedJointStr(
+        "box_fixed_world_joint", gz::math::Pose3d::Zero));
+    ASSERT_TRUE(errors.empty()) << errors.front();
+    ASSERT_NE(nullptr, root.Model());
+    world->ConstructModel(*root.Model());
+
+    world->Step(output, state, input);
+    // box fixed to world overlaps with static box and ground plane
+    // verify there are still no contacts.
+    contacts = world->GetContactsFromLastStep();
+    EXPECT_EQ(0u, contacts.size());
+  }
+}
+
 int main(int argc, char *argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);

test/common_test/detachable_joint.cc

@@ -187,7 +187,14 @@ TYPED_TEST(DetachableJointTest, CorrectAttachmentPoints)
       // ground.
       auto frameDataC1L1 = cylinder1_link1->FrameDataRelativeToWorld();
       auto frameDataC2L2 = cylinder2_link2->FrameDataRelativeToWorld();
-      EXPECT_NEAR(0.15, frameDataC1L1.pose.translation().z(), 1e-2);
+#ifdef __APPLE__
+      // Disable check for dartsim plugin on homebrew,
+      // see https://github.com/gazebosim/gz-physics/issues/612.
+      if (this->PhysicsEngineName(name) != "dartsim")
+#endif
+      {
+        EXPECT_NEAR(0.15, frameDataC1L1.pose.translation().z(), 1e-2);
+      }
       EXPECT_NEAR(0.05, frameDataC2L2.pose.translation().z(), 1e-2);
     }
   }

test/common_test/joint_features.cc

@@ -1040,8 +1040,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach)
     auto frameDataModel1Body = model1Body->FrameDataRelativeToWorld();
     auto frameDataModel2Body = model2Body->FrameDataRelativeToWorld();
 
-    const gz::math::Pose3d initialModel1Pose(0, 0, 0.25, 0, 0, 0);
-    const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0);
+    const gz::math::Pose3d initialModel1Pose(0, 0, 0.25, 0, 0, 0.1);
+    const gz::math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0.2);
 
     EXPECT_EQ(initialModel1Pose,
               gz::math::eigen3::convert(frameDataModel1Body.pose));
@@ -1133,14 +1133,14 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach)
 
     // After a while, body2 should reach the ground and come to a stop
     std::size_t stepCount = 0u;
-    const std::size_t maxNumSteps = 1000u;
+    const std::size_t maxNumSteps = 2000u;
     while (stepCount++ < maxNumSteps)
     {
       world->Step(output, state, input);
       frameDataModel2Body = model2Body->FrameDataRelativeToWorld();
       // Expected Z height of model2 is 0.75 when both boxes are stacked on top
       // of each other since each is 0.5 high.
-      if (fabs(frameDataModel2Body.pose.translation().z() - 0.75) < 2e-2 &&
+      if (fabs(frameDataModel2Body.pose.translation().z() - 0.75) < 1e-2 &&
           fabs(frameDataModel2Body.linearVelocity.z()) < 1e-3)
       {
         break;

test/common_test/worlds/joint_across_models.sdf

@@ -29,7 +29,7 @@
     </model>
 
     <model name="M1">
-      <pose>0 0 0.25 0 0.0 0</pose>
+      <pose>0 0 0.25 0 0.0 0.1</pose>
       <link name="body">
         <inertial>
           <inertia>
@@ -65,7 +65,7 @@
       </link>
     </model>
     <model name="M2">
-      <pose>0 0 3.0 0 0.0 0</pose>
+      <pose>0 0 3.0 0 0.0 0.2</pose>
       <link name="body">
         <inertial>
           <inertia>