From a86da627d5a53f16a0d59a7b85c5df21737b5ee5 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 22 Mar 2024 17:12:42 -0700 Subject: [PATCH 1/3] [bullet-featherstone] Fix attaching fixed joint (#610) Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 5 ++++- test/common_test/joint_features.cc | 8 ++++---- test/common_test/worlds/joint_across_models.sdf | 4 ++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index e53dddd9b..980313a7b 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/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()); } } diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 346821bb6..087ddd727 100644 --- a/test/common_test/joint_features.cc +++ b/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; diff --git a/test/common_test/worlds/joint_across_models.sdf b/test/common_test/worlds/joint_across_models.sdf index a4308581d..c221a7aaf 100644 --- a/test/common_test/worlds/joint_across_models.sdf +++ b/test/common_test/worlds/joint_across_models.sdf @@ -29,7 +29,7 @@ - 0 0 0.25 0 0.0 0 + 0 0 0.25 0 0.0 0.1 @@ -65,7 +65,7 @@ - 0 0 3.0 0 0.0 0 + 0 0 3.0 0 0.0 0.2 From d616379c6875f04527b1761ecdd7633155e17138 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 29 Mar 2024 13:32:03 -0700 Subject: [PATCH 2/3] Disable check in DetachableJointTest, CorrectAttachmentPoints for dartsim plugin on macOS (#613) Signed-off-by: Ian Chen --- test/common_test/detachable_joint.cc | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/test/common_test/detachable_joint.cc b/test/common_test/detachable_joint.cc index fad71648d..1d94ed021 100644 --- a/test/common_test/detachable_joint.cc +++ b/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); } } From 2bc19ad0d7ca34f2150c765e8cce5d712ba117bd Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 4 Apr 2024 10:27:58 -0700 Subject: [PATCH 3/3] [bullet-featherstone] Ignore collision between static objects and objects with world joint (#611) * ignore collision between static link and base link with world joint Signed-off-by: Ian Chen * check link with zero dof Signed-off-by: Ian Chen * update test Signed-off-by: Ian Chen --------- Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 19 +++- test/common_test/collisions.cc | 125 +++++++++++++++++++++++++ 2 files changed, 143 insertions(+), 1 deletion(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index cd5d0d6dd..832839b84 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1205,7 +1205,24 @@ bool SDFFeatures::AddSdfCollision( auto *world = this->ReferenceInterface(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(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(), diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index f78495126..db2567e41 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -23,6 +24,7 @@ #include "test/Resources.hh" #include "test/TestLibLoader.hh" +#include "Worlds.hh" #include #include @@ -30,14 +32,18 @@ #include #include #include +#include #include #include #include #include +#include #include #include +#include + template 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; + +TEST_F(CollisionStaticTestFeaturesList, StaticCollisions) +{ + auto getBoxStaticStr = [](const std::string &_name, + const gz::math::Pose3d &_pose) + { + std::stringstream modelStaticStr; + modelStaticStr << R"( + + + )"; + modelStaticStr << _pose; + modelStaticStr << R"( + + + + 1 1 1 + + + + true + + )"; + return modelStaticStr.str(); + }; + + auto getBoxFixedJointStr = [](const std::string &_name, + const gz::math::Pose3d &_pose) + { + std::stringstream modelFixedJointStr; + modelFixedJointStr << R"( + + + )"; + modelFixedJointStr << _pose; + modelFixedJointStr << R"( + + + + 1 1 1 + + + + + world + body + + + )"; + 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::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);