Merge branch 'gz-physics7' into bullet_constraint_force_torque

gazebosim · Aug 12, 2024 · 8e65812 · 8e65812
2 parents 1432013 + a068d3f
commit 8e65812
Show file tree

Hide file tree

Showing 9 changed files with 281 additions and 7 deletions.
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
@@ -1,4 +1,4 @@
 # More info:
 # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners
 
-*                     @azeey @mxgrey @scpeters
+*                     @azeey @scpeters
diff --git a/Changelog.md b/Changelog.md
@@ -154,6 +154,66 @@
 
 ## Gazebo Physics 6.x
 
+### Gazebo Physics 6.6.0 (2024-06-11)
+
+1. dartsim: optimize picking contact points with ODE collision detector
+    * [Pull request #584](https://github.com/gazebosim/gz-physics/pull/584)
+
+1. Fix windows compiler warning
+    * [Pull request #629](https://github.com/gazebosim/gz-physics/pull/629)
+
+1. Disable test failing due to ODE/libccd
+    * [Pull request #621](https://github.com/gazebosim/gz-physics/pull/621)
+
+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. Revert "bazel: updates for garden (#513)"
+    * [Pull request #513](https://github.com/gazebosim/gz-physics/pull/513)
+
+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. Reduce error to debug messsage for mesh construction (#581)
+    * [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 6.5.1 (2023-09-26)
 
 1. joint_features test: reduce console spam

diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzOdeCollisionDetector.cc
@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <unordered_map>
+#include <utility>
 
 #include <dart/collision/CollisionObject.hpp>
 
@@ -92,21 +93,45 @@ void GzOdeCollisionDetector::LimitCollisionPairMaxContacts(
   auto allContacts = _result->getContacts();
   _result->clear();
 
+
+  if (this->maxCollisionPairContacts == 0u)
+    return;
+
+  // A map of collision pairs and their contact info
+  // Contact info is stored in std::pair. The elements are:
+  // <contact count, index of last contact point (in _result)>
   std::unordered_map<dart::collision::CollisionObject *,
-      std::unordered_map<dart::collision::CollisionObject *, std::size_t>>
+      std::unordered_map<dart::collision::CollisionObject *,
+      std::pair<std::size_t, std::size_t>>>
       contactMap;
 
   for (auto &contact : allContacts)
   {
-    auto &count =
-      contactMap[contact.collisionObject1][contact.collisionObject2];
+    auto &[count, lastContactIdx] =
+        contactMap[contact.collisionObject1][contact.collisionObject2];
     count++;
-    auto &otherCount =
+    auto &[otherCount, otherLastContactIdx] =
       contactMap[contact.collisionObject2][contact.collisionObject1];
+
     std::size_t total =  count + otherCount;
     if (total <= this->maxCollisionPairContacts)
     {
+      if (total == this->maxCollisionPairContacts)
+      {
+        lastContactIdx = _result->getNumContacts();
+        otherLastContactIdx = lastContactIdx;
+      }
       _result->addContact(contact);
     }
+    else
+    {
+      // If too many contacts were generated, replace the last contact point
+      // of the collision pair with one that has a larger penetration depth
+      auto &c = _result->getContact(lastContactIdx);
+      if (contact.penetrationDepth > c.penetrationDepth)
+      {
+        c = contact;
+      }
+    }
   }
 }
diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
@@ -674,6 +674,7 @@ Identity SDFFeatures::ConstructSdfLink(
 
   bodyProperties.mInertia.setLocalCOM(localCom);
 
+  bodyProperties.mGravityMode = _sdfLink.EnableGravity();
 
   dart::dynamics::FreeJoint::Properties jointProperties;
   jointProperties.mName = bodyProperties.mName + "_FreeJoint";

diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh
@@ -29,6 +29,8 @@ inline std::string CommonTestWorld(const std::string &_world)
 
 namespace common_test::worlds
 {
+const auto kCollisionPairContactPointSdf =
+  CommonTestWorld("collision_pair_contact_point.sdf");
 const auto kContactSdf = CommonTestWorld("contact.sdf");
 const auto kDetachableJointWorld = CommonTestWorld("detachable_joint.world");
 const auto kEmptySdf = CommonTestWorld("empty.sdf");

diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc
@@ -228,9 +228,13 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts)
 // The features that an engine must have to be loaded by this loader.
 struct FeaturesCollisionPairMaxContacts : gz::physics::FeatureList<
   gz::physics::sdf::ConstructSdfWorld,
-  gz::physics::GetContactsFromLastStepFeature,
+  gz::physics::CollisionPairMaxContacts,
+  gz::physics::FindFreeGroupFeature,
   gz::physics::ForwardStep,
-  gz::physics::CollisionPairMaxContacts
+  gz::physics::FreeGroupFrameSemantics,
+  gz::physics::GetContactsFromLastStepFeature,
+  gz::physics::GetModelFromWorld,
+  gz::physics::SetFreeGroupWorldPose
 > {};
 
 template <class T>
@@ -281,6 +285,92 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest,
   }
 }
 
+/////////////////////////////////////////////////
+TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest,
+    CollisionPairMaxContactsSelection)
+{
+  for (const std::string &name : this->pluginNames)
+  {
+    auto world = LoadPluginAndWorld<FeaturesCollisionPairMaxContacts>(
+      this->loader,
+      name,
+      common_test::worlds::kCollisionPairContactPointSdf);
+    auto checkedOutput = StepWorld<FeaturesCollisionPairMaxContacts>(
+        world, true, 1).first;
+    EXPECT_TRUE(checkedOutput);
+
+    // Verify initial pose
+    const gz::math::Pose3d initialPose = gz::math::Pose3d::Zero;
+    auto ellipsoid = world->GetModel("ellipsoid");
+    ASSERT_NE(nullptr, ellipsoid);
+    auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup();
+    ASSERT_NE(nullptr, ellipsoidFreeGroup);
+    auto box = world->GetModel("box");
+    ASSERT_NE(nullptr, box);
+    auto boxFreeGroup = box->FindFreeGroup();
+    ASSERT_NE(nullptr, boxFreeGroup);
+    auto ellipsoidFrameData = ellipsoidFreeGroup->FrameDataRelativeToWorld();
+    auto boxFrameData = boxFreeGroup->FrameDataRelativeToWorld();
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(ellipsoidFrameData.pose));
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(boxFrameData.pose));
+
+    // Get all contacts between box and ellipsoid
+    auto contacts = world->GetContactsFromLastStep();
+    EXPECT_EQ(std::numeric_limits<std::size_t>::max(),
+              world->GetCollisionPairMaxContacts());
+    EXPECT_GT(contacts.size(), 30u);
+
+    // Find contact point with max penetration depth
+    double maxDepth = 0;
+    for (const auto &contact : contacts)
+    {
+      const auto* extraContactData =
+          contact.template Query<
+          gz::physics::World3d<
+          FeaturesCollisionPairMaxContacts>::ExtraContactData>();
+      ASSERT_NE(nullptr, extraContactData);
+      if (extraContactData->depth > maxDepth)
+        maxDepth = extraContactData->depth;
+    }
+    EXPECT_GT(maxDepth, 0.0);
+
+    // Reset pose back to initial pose
+    ellipsoidFreeGroup->SetWorldPose(
+      gz::math::eigen3::convert(initialPose));
+    boxFreeGroup->SetWorldPose(
+      gz::math::eigen3::convert(initialPose));
+    ellipsoidFrameData = ellipsoidFreeGroup->FrameDataRelativeToWorld();
+    boxFrameData = boxFreeGroup->FrameDataRelativeToWorld();
+
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(ellipsoidFrameData.pose));
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(boxFrameData.pose));
+
+    // Set max contact between collision pairs to be 1
+    world->SetCollisionPairMaxContacts(1u);
+    EXPECT_EQ(1u, world->GetCollisionPairMaxContacts());
+    checkedOutput = StepWorld<FeaturesCollisionPairMaxContacts>(
+        world, true, 1).first;
+    EXPECT_TRUE(checkedOutput);
+
+    contacts = world->GetContactsFromLastStep();
+    EXPECT_EQ(1u, contacts.size());
+
+    // Verify that the physics engine picked the contact with max penetration
+    // depth
+    auto contact = contacts[0];
+    const auto* extraContactData =
+        contact.template Query<
+        gz::physics::World3d<
+        FeaturesCollisionPairMaxContacts>::ExtraContactData>();
+    ASSERT_NE(nullptr, extraContactData);
+    EXPECT_FLOAT_EQ(maxDepth, extraContactData->depth);
+  }
+}
+
 // The features that an engine must have to be loaded by this loader.
 struct FeaturesStep : gz::physics::FeatureList<
   gz::physics::sdf::ConstructSdfWorld,

diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc
@@ -123,6 +123,12 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures)
     auto link = model->GetLink(0);
     ASSERT_NE(nullptr, link);
 
+    auto modelNoGravity = world->GetModel("sphere_no_gravity");
+    ASSERT_NE(nullptr, modelNoGravity);
+
+    auto linkNoGravity = modelNoGravity->GetLink(0);
+    ASSERT_NE(nullptr, linkNoGravity);
+
     AssertVectorApprox vectorPredicate6(1e-6);
 
     // initial link pose
@@ -175,6 +181,16 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures)
       EXPECT_PRED_FORMAT2(vectorPredicate2,
                           Eigen::Vector3d(0.5, 0, 2.5),
                           pos);
+
+      if (this->PhysicsEngineName(name) == "dartsim")
+      {
+        // pose for link without gravity should not change
+        Eigen::Vector3d posNoGravity = linkNoGravity->FrameDataRelativeToWorld()
+                            .pose.translation();
+        EXPECT_PRED_FORMAT2(vectorPredicate2,
+                            Eigen::Vector3d(10, 10, 10),
+                            posNoGravity);
+      }
     }
   }
 }

diff --git a/test/common_test/worlds/collision_pair_contact_point.sdf b/test/common_test/worlds/collision_pair_contact_point.sdf
@@ -0,0 +1,68 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+
+   <model name="box">
+      <static>true</static>
+      <pose>0 0 0.0 0 0 0</pose>
+      <link name="box_link">
+        <inertial>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="box_collision">
+          <geometry>
+            <box>
+              <size>100 100 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="box_visual">
+          <geometry>
+            <box>
+              <size>100 100 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+
+    <model name="ellipsoid">
+      <pose>0 -0 0.0 0 0 0</pose>
+      <link name="ellipsoid_link">
+        <inertial>
+          <inertia>
+            <ixx>0.068</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.058</iyy>
+            <iyz>0</iyz>
+            <izz>0.026</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="ellipsoid_collision">
+          <geometry>
+            <ellipsoid>
+              <radii>0.2 0.3 0.5</radii>
+            </ellipsoid>
+          </geometry>
+        </collision>
+        <visual name="ellipsoid_visual">
+          <geometry>
+            <ellipsoid>
+              <radii>0.2 0.3 0.5</radii>
+            </ellipsoid>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>
diff --git a/test/common_test/worlds/falling.world b/test/common_test/worlds/falling.world
@@ -71,5 +71,17 @@
         </visual>
       </link>
     </model>
+    <model name='sphere_no_gravity'>
+      <link name='link'>
+        <gravity>false</gravity>
+        <pose>10 10 10 0 0 0</pose>
+        <visual name='visual'>
+          <geometry><sphere><radius>1</radius></sphere></geometry>
+        </visual>
+        <collision name='collision'>
+          <geometry><sphere><radius>1</radius></sphere></geometry>
+        </collision>
+      </link>
+    </model>
   </world>
 </sdf>