From 7d30e47fb61fe4db9cba460717a3910df9deabee Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 14 Mar 2024 23:43:11 -0500 Subject: [PATCH 01/10] Prepare for 7.1.0 (#607) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 51 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e6350c16..32c31fbc8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-physics7 VERSION 7.0.0) +project(gz-physics7 VERSION 7.1.0) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index 0b80efede..ee8348745 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,56 @@ ## Gazebo Physics 7.x +### 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 From a86da627d5a53f16a0d59a7b85c5df21737b5ee5 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 22 Mar 2024 17:12:42 -0700 Subject: [PATCH 02/10] [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 03/10] 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 04/10] [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); From 4e400e0e12c3444c432d7c03f6e66f9fa4c82d4f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 8 Apr 2024 18:14:24 -0700 Subject: [PATCH 05/10] Disable test failing due to ODE/libccd (#621) Part of #620. Signed-off-by: Steve Peters --- test/common_test/joint_features.cc | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 087ddd727..b19f0e980 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -966,9 +966,23 @@ TYPED_TEST(JointFeaturesDetachTest, JointDetach) // sanity check on velocity values EXPECT_LT(1e-5, upperLinkLinearVelocity.Z()); EXPECT_GT(-0.03, upperLinkAngularVelocity.X()); - EXPECT_NEAR(0.0, upperLinkLinearVelocity.X(), 1e-6); +#ifdef __APPLE__ + // Disable some expectations for dartsim plugin on homebrew, + // see https://github.com/gazebosim/gz-physics/issues/620. + if (this->PhysicsEngineName(name) != "dartsim") +#endif + { + EXPECT_NEAR(0.0, upperLinkLinearVelocity.X(), 1e-6); + } EXPECT_NEAR(0.0, upperLinkLinearVelocity.Y(), 1e-6); - EXPECT_NEAR(0.0, upperLinkAngularVelocity.Y(), 1e-6); +#ifdef __APPLE__ + // Disable some expectations for dartsim plugin on homebrew, + // see https://github.com/gazebosim/gz-physics/issues/620. + if (this->PhysicsEngineName(name) != "dartsim") +#endif + { + EXPECT_NEAR(0.0, upperLinkAngularVelocity.Y(), 1e-6); + } EXPECT_NEAR(0.0, upperLinkAngularVelocity.Z(), 1e-6); upperJoint->Detach(); From 0fe1ae99d0d7f4ce3cfe7f6c85748778aa2f5990 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 10 Apr 2024 10:19:54 -0500 Subject: [PATCH 06/10] Use relative install paths for plugin shared libraries (#616) Signed-off-by: Addisu Z. Taddese Co-authored-by: Ian Chen --- bullet-featherstone/CMakeLists.txt | 4 ++-- bullet/CMakeLists.txt | 4 ++-- dartsim/CMakeLists.txt | 4 ++-- tpe/plugin/CMakeLists.txt | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/bullet-featherstone/CMakeLists.txt b/bullet-featherstone/CMakeLists.txt index c8ee36f41..fbae005e4 100644 --- a/bullet-featherstone/CMakeLists.txt +++ b/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() diff --git a/bullet/CMakeLists.txt b/bullet/CMakeLists.txt index 134284448..f5553822c 100644 --- a/bullet/CMakeLists.txt +++ b/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() diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index fbf06aab7..1a39a989c 100644 --- a/dartsim/CMakeLists.txt +++ b/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 diff --git a/tpe/plugin/CMakeLists.txt b/tpe/plugin/CMakeLists.txt index 573fe8adb..636a2b2c7 100644 --- a/tpe/plugin/CMakeLists.txt +++ b/tpe/plugin/CMakeLists.txt @@ -32,7 +32,7 @@ target_link_libraries(${tpe_plugin} ) # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir -install(TARGETS ${tpe_plugin} DESTINATION ${GZ_PHYSICS_ENGINE_INSTALL_DIR}) +install(TARGETS ${tpe_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, @@ -47,7 +47,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() gz_build_tests( From bcacf85e3e25961263725f424191027951a03430 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 10 Apr 2024 14:26:54 -0500 Subject: [PATCH 07/10] Prepare for 7.2.0 (#622) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 32c31fbc8..aa41e2dcb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-physics7 VERSION 7.1.0) +project(gz-physics7 VERSION 7.2.0) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index ee8348745..b2cbe43bd 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,22 @@ ## 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 From 624dc27bfa009a46657ef6da621a5d98e9f8972e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 11 Apr 2024 00:38:04 -0700 Subject: [PATCH 08/10] Revert "Disable check in DetachableJointTest, CorrectAttachmentPoints for dartsim plugin on macOS (#613)" (#615) This reverts commit d616379c6875f04527b1761ecdd7633155e17138. Signed-off-by: Steve Peters Co-authored-by: Ian Chen --- test/common_test/detachable_joint.cc | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/test/common_test/detachable_joint.cc b/test/common_test/detachable_joint.cc index 1d94ed021..fad71648d 100644 --- a/test/common_test/detachable_joint.cc +++ b/test/common_test/detachable_joint.cc @@ -187,14 +187,7 @@ TYPED_TEST(DetachableJointTest, CorrectAttachmentPoints) // ground. auto frameDataC1L1 = cylinder1_link1->FrameDataRelativeToWorld(); auto frameDataC2L2 = cylinder2_link2->FrameDataRelativeToWorld(); -#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.15, frameDataC1L1.pose.translation().z(), 1e-2); EXPECT_NEAR(0.05, frameDataC2L2.pose.translation().z(), 1e-2); } } From d89c026930f09c2e1f5960a2c85fa05b9188e84f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 15 Apr 2024 13:33:25 -0700 Subject: [PATCH 09/10] bullet-featherstone: fix SetWorldPose with off-diagonal moment of inertia (#623) Signed-off-by: Ian Chen --- bullet-featherstone/src/FreeGroupFeatures.cc | 3 +- test/common_test/free_joint_features.cc | 76 ++++++++++++++++++++ 2 files changed, 78 insertions(+), 1 deletion(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 22e219528..4fd79f57f 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -93,7 +93,8 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const auto *model = this->ReferenceInterface(_groupID); if (model) { - model->body->setBaseWorldTransform(convertTf(_pose)); + model->body->setBaseWorldTransform( + convertTf(_pose * model->baseInertiaToLinkFrame.inverse())); } } diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index 7fe3b2633..a896efcb7 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -240,6 +240,82 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) } } +TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset) +{ + const std::string modelStr = R"( + + + 1 2 3.0 0 0 0 + + + -0.00637 -0.008 0.13254 0 0 0 + + 0.01331127 + -0.00030365 + -0.00034148 + 0.01157659 + 0.00088073 + 0.00378028 + + 1.50251902 + + + + + 0.1 + + + + + + )"; + + for (const std::string &name : pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + sdf::Errors errors = root.Load( + common_test::worlds::kGroundSdf); + EXPECT_EQ(0u, errors.size()) << errors; + + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *sdfWorld = root.WorldByIndex(0); + ASSERT_NE(nullptr, sdfWorld); + + auto world = engine->ConstructWorld(*sdfWorld); + ASSERT_NE(nullptr, world); + + // create the model + errors = root.LoadSdfString(modelStr); + ASSERT_TRUE(errors.empty()) << errors; + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + auto model = world->GetModel("box"); + ASSERT_NE(nullptr, model); + auto link = model->GetLink("link"); + ASSERT_NE(nullptr, link); + auto frameDataLink = link->FrameDataRelativeToWorld(); + EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), + gz::math::eigen3::convert(frameDataLink.pose)); + + // get free group and set new pose + auto freeGroup = model->FindFreeGroup(); + ASSERT_NE(nullptr, freeGroup); + gz::math::Pose3d newPose(4, 5, 6, 0, 0, 1.57); + freeGroup->SetWorldPose( + gz::math::eigen3::convert(newPose)); + frameDataLink = link->FrameDataRelativeToWorld(); + EXPECT_EQ(newPose, + gz::math::eigen3::convert(frameDataLink.pose)); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); From d0a6e507210cb17f39a4d615fcdf75adfeda4838 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 8 Apr 2024 20:34:20 +0000 Subject: [PATCH 10/10] backport bullet-featherstone solver iters Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.cc | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index d03794932..66f713223 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -45,10 +45,15 @@ WorldInfo::WorldInfo(std::string name_) // By default a large impulse is applied when collisions penetrate // which causes unstable behavior. Bullet featherstone does not support - // configuring split impulse and penetration threshold parameters. Instead the - // penentration impulse depends on the erp2 parameter so set to a small value - // (default is 0.2). + // configuring split impulse and penetration threshold parameters. Instead + // the penentration impulse depends on the erp2 parameter so set to a small + // value (default in bullet is 0.2). this->world->getSolverInfo().m_erp2 = btScalar(0.002); + + // Set solver iterations to the same as the default value in SDF, + // //world/physics/solver/bullet/iters + // (default in bullet is 10) + this->world->getSolverInfo().m_numIterations = 50u; } } // namespace bullet_featherstone