diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5534c1d87..fa88665d8 100644 --- 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 index d2166a972..bc449953d 100644 --- 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 index 228ee9562..6a81b4e06 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzOdeCollisionDetector.cc @@ -17,6 +17,7 @@ #include #include +#include #include @@ -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: + // std::unordered_map> + std::unordered_map>> 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 index 3993dceb6..f06e108d0 100644 --- 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 index 326ac201d..37d77efd7 100644 --- 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 index bc30de759..d5537f1cd 100644 --- 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 @@ -281,6 +285,92 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, } } +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, + CollisionPairMaxContactsSelection) +{ + for (const std::string &name : this->pluginNames) + { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kCollisionPairContactPointSdf); + auto checkedOutput = StepWorld( + 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::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( + 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 index e1e68c0f0..f1178df2d 100644 --- 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 new file mode 100644 index 000000000..74377d4b6 --- /dev/null +++ b/test/common_test/worlds/collision_pair_contact_point.sdf @@ -0,0 +1,68 @@ + + + + + + true + 0 0 0.0 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 100 100 1 + + + + + + + 100 100 1 + + + + + + + + 0 -0 0.0 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + + + + diff --git a/test/common_test/worlds/falling.world b/test/common_test/worlds/falling.world index 1d2ea2f43..1b3e32273 100644 --- a/test/common_test/worlds/falling.world +++ b/test/common_test/worlds/falling.world @@ -71,5 +71,17 @@ + + + false + 10 10 10 0 0 0 + + 1 + + + 1 + + +