Merge branch 'gz-physics7' into scpeters/merge_7_main

Changelog.md

@@ -4,6 +4,74 @@
 
 ## Gazebo Physics 7.x
 
+### Gazebo Physics 7.2.0 (2024-04-10)
+
+1. Use relative install paths for plugin shared libraries
+    * [Pull request #616](https://github.com/gazebosim/gz-physics/pull/616)
+
+1. Disable test failing due to ODE/libccd
+    * [Pull request #621](https://github.com/gazebosim/gz-physics/pull/621)
+
+1. bullet-featherstone: Ignore collision between static objects and objects with world joint
+    * [Pull request #611](https://github.com/gazebosim/gz-physics/pull/611)
+
+1. Disable check in DetachableJointTest, CorrectAttachmentPoints for dartsim plugin on macOS
+    * [Pull request #613](https://github.com/gazebosim/gz-physics/pull/613)
+
+1. bullet-featherstone: Fix attaching fixed joint
+    * [Pull request #610](https://github.com/gazebosim/gz-physics/pull/610)
+
+### Gazebo Physics 7.1.0 (2024-03-14)
+
+1. bullet-featherstone: Improve mesh collision stability
+    * [Pull request #600](https://github.com/gazebosim/gz-physics/pull/600)
+
+1. bullet-featherstone: Support nested models
+    * [Pull request #574](https://github.com/gazebosim/gz-physics/pull/574)
+
+1. Garden test cleanup
+    * [Pull request #587](https://github.com/gazebosim/gz-physics/pull/587)
+
+1. Support setting max contacts in dartsim's ODE collision detector
+    * [Pull request #582](https://github.com/gazebosim/gz-physics/pull/582)
+
+1. Get bullet version from cmake instead of API
+    * [Pull request #591](https://github.com/gazebosim/gz-physics/pull/591)
+
+1. Update CI badges in README
+    * [Pull request #583](https://github.com/gazebosim/gz-physics/pull/583)
+
+1. Reduce error to debug messsage for mesh construction
+    * [Pull request #581](https://github.com/gazebosim/gz-physics/pull/581)
+
+1. bullet-featherstone: Set collision spinning friction
+    * [Pull request #579](https://github.com/gazebosim/gz-physics/pull/579)
+
+1. Infrastructure
+    * [Pull request #578](https://github.com/gazebosim/gz-physics/pull/578)
+    * [Pull request #572](https://github.com/gazebosim/gz-physics/pull/572)
+
+1. dartsim: fix handling inertia matrix pose rotation
+    * [Pull request #351](https://github.com/gazebosim/gz-physics/pull/351)
+
+1. bullet-featherstone: fix setting angular velocity
+    * [Pull request #567](https://github.com/gazebosim/gz-physics/pull/567)
+
+1. bullet-featherstone: support off-diagonal inertia
+    * [Pull request #544](https://github.com/gazebosim/gz-physics/pull/544)
+
+1. bullet-featherstone: Fix how links are flattened in  ConstructSdfModel
+    * [Pull request #562](https://github.com/gazebosim/gz-physics/pull/562)
+
+1. Add sample ctest cmds to tutorial
+    * [Pull request #566](https://github.com/gazebosim/gz-physics/pull/566)
+
+1. Add a test to verify behavior of detachable joints
+    * [Pull request #563](https://github.com/gazebosim/gz-physics/pull/563)
+
+1. Use correct link indicies when constructing fixed constraints
+    * [Pull request #530](https://github.com/gazebosim/gz-physics/pull/530)
+
 ### Gazebo Physics 7.0.0 (2023-09-29)
 
 1. dartsim: Fix sign convention error with contact surface motion velocities

bullet-featherstone/CMakeLists.txt

@@ -25,7 +25,7 @@ target_link_libraries(${bullet_plugin}
     gz-math${GZ_MATH_VER}::eigen3)
 
 # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir
-install(TARGETS ${bullet_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+install(TARGETS ${bullet_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 
 # The library created by `gz_add_component` includes the ign-physics version
 # (i.e. libgz-physics1-name-plugin.so), but for portability,
@@ -40,5 +40,5 @@ if (WIN32)
       ${GZ_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})")
 else()
   EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned})
-  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 endif()

bullet-featherstone/src/FreeGroupFeatures.cc

@@ -93,7 +93,8 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
   const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
   if (model)
   {
-    model->body->setBaseWorldTransform(convertTf(_pose));
+    model->body->setBaseWorldTransform(
+        convertTf(_pose * model->baseInertiaToLinkFrame.inverse()));
   }
 }
 

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(),

bullet/CMakeLists.txt

@@ -25,7 +25,7 @@ target_link_libraries(${bullet_plugin}
     gz-math${GZ_MATH_VER}::eigen3)
 
 # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir
-install(TARGETS ${bullet_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+install(TARGETS ${bullet_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 
 # The library created by `gz_add_component` includes the gz-physics version
 # (i.e. libgz-physics1-name-plugin.so), but for portability,
@@ -40,5 +40,5 @@ if (WIN32)
       ${GZ_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})")
 else()
   EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned})
-  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 endif()

dartsim/CMakeLists.txt

@@ -55,7 +55,7 @@ else()
 endif()
 
 # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir
-install(TARGETS ${dartsim_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+install(TARGETS ${dartsim_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 
 # The library created by `gz_add_component` includes the gz-physics version
 # (i.e. libgz-physics1-name-plugin.so), but for portability,
@@ -70,7 +70,7 @@ if (WIN32)
       ${GZ_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})")
 else()
   EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned})
-  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR})
+  INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
 endif()
 
 # Testing

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);