From 9faf6b55975031e249a0a4f3ee0fcfc4cc5c0c72 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 1 Sep 2023 13:08:17 -0500 Subject: [PATCH 1/5] Prepare for 5.3.2 release (#536) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 12 ++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b6ce32f39..c4236e18d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-physics5 VERSION 5.3.1) +project(ignition-physics5 VERSION 5.3.2) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 889ed3760..c158d9fba 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,17 @@ ## Ignition Physics 5.x +### Ignition Physics 5.3.2 (2023-09-01) + +1. Fix a crash due to an invalid pointer + * [Pull request #486](https://github.com/gazebosim/gz-physics/pull/486) + +1. Infrastructure + * [Pull request #490](https://github.com/gazebosim/gz-physics/pull/490) + * [Pull request #488](https://github.com/gazebosim/gz-physics/pull/488) + +1. Rename COPYING to LICENSE + * [Pull request #487](https://github.com/gazebosim/gz-physics/pull/487) + ### Ignition Physics 5.3.1 (2023-02-16) 1. Fix memory corruption due to faulty refcount tracking From 2c238fe87b7c5ebd3d1ba37784db39ce93a6f143 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 22 Aug 2023 14:41:57 -0500 Subject: [PATCH 2/5] Unregister collision detectors when the darstim plugin is unloaded (#529) Fixes a segfault that occurs due to destructors being removed from memory before they're called. --------- Signed-off-by: Addisu Z. Taddese --- dartsim/src/plugin.cc | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/dartsim/src/plugin.cc b/dartsim/src/plugin.cc index 778af55c5..e184522b7 100644 --- a/dartsim/src/plugin.cc +++ b/dartsim/src/plugin.cc @@ -59,6 +59,31 @@ class Plugin : public virtual SimulationFeatures, public virtual WorldFeatures { }; +namespace { + +// This is done as a partial fix for +// https://github.com/gazebosim/gz-physics/issues/442. The issue seems like the +// destructors for the concrete collision detectors get unloaded and deleted +// from memory before the destructors run. When it's time to actually call the +// destructors, a segfault is generated. +// +// It's not clear why the destructors are deleted prematurely. It might be a +// compiler optimization in new compiler versions. +// +// The solution here is to call the `unregisterAllCreators` function from the +// plugins translation unit in the hopes that it will force the compiler to keep +// the destructors. +struct UnregisterCollisionDetectors +{ + ~UnregisterCollisionDetectors() + { + dart::collision::CollisionDetector::getFactory()->unregisterAllCreators(); + } +}; + +UnregisterCollisionDetectors unregisterAtUnload; +} + IGN_PHYSICS_ADD_PLUGIN(Plugin, FeaturePolicy3d, DartsimFeatures) } From 6a21a96b8264ec4aa55c6e76f7fd606aae94ddf7 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 20 Sep 2023 10:19:37 -0500 Subject: [PATCH 3/5] Cleaning up bullet memory use issues (#539) * Cleaning up bullet memory use issues * Fix Windows warning * Fix same issue in bullet (non-featherstone) implementation --------- Signed-off-by: Michael Carroll Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- bullet-featherstone/src/Base.hh | 30 ++++++++++++++++++-- bullet-featherstone/src/FreeGroupFeatures.cc | 14 +++++---- bullet-featherstone/src/JointFeatures.cc | 8 +++--- bullet-featherstone/src/SDFFeatures.cc | 21 +++++++------- bullet/src/Base.hh | 20 +++++++++++++ 5 files changed, 70 insertions(+), 23 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index cc786b61a..184d958c2 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -165,9 +166,10 @@ struct JointInfo Identity model; // This field gets set by AddJoint std::size_t indexInGzModel = 0; - btMultiBodyJointMotor* motor = nullptr; btScalar effort = 0; + std::shared_ptr motor = nullptr; + std::shared_ptr jointLimits = nullptr; std::shared_ptr fixedConstraint = nullptr; std::shared_ptr jointFeedback = nullptr; }; @@ -355,6 +357,30 @@ class Base : public Implements3d> return this->GenerateIdentity(id, joint); } + public: ~Base() override { + // The order of destruction between meshesGImpact and triangleMeshes is + // important. + this->meshesGImpact.clear(); + this->triangleMeshes.clear(); + + this->joints.clear(); + + for (const auto &[k, link] : links) + { + auto *model = this->ReferenceInterface(link->model); + auto *world = this->ReferenceInterface(model->world); + if (link->collider != nullptr) + { + world->world->removeCollisionObject(link->collider.get()); + } + } + + this->collisions.clear(); + this->links.clear(); + this->models.clear(); + this->worlds.clear(); + } + public: using WorldInfoPtr = std::shared_ptr; public: using ModelInfoPtr = std::shared_ptr; public: using LinkInfoPtr = std::shared_ptr; @@ -367,8 +393,6 @@ class Base : public Implements3d> public: std::unordered_map collisions; public: std::unordered_map joints; - // Note, the order of triangleMeshes and meshesGImpact is important. Reversing - // the order causes segfaults on macOS during destruction. public: std::vector> triangleMeshes; public: std::vector> meshesGImpact; }; diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index ba05b4982..5fd18e9f3 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -17,6 +17,8 @@ #include "FreeGroupFeatures.hh" +#include + namespace gz { namespace physics { namespace bullet_featherstone { @@ -64,7 +66,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); - if(model) + if (model != nullptr) { // Set angular velocity the each one of the joints of the model for (const auto& jointID : model->jointEntityIds) @@ -72,15 +74,15 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( auto jointInfo = this->joints[jointID]; if (!jointInfo->motor) { - auto modelInfo = this->ReferenceInterface(jointInfo->model); - jointInfo->motor = new btMultiBodyJointMotor( + auto *modelInfo = this->ReferenceInterface(jointInfo->model); + jointInfo->motor = std::make_shared( modelInfo->body.get(), std::get(jointInfo->identifier).indexInBtModel, 0, - 0, - jointInfo->effort); + static_cast(0), + static_cast(jointInfo->effort)); auto *world = this->ReferenceInterface(modelInfo->world); - world->world->addMultiBodyConstraint(jointInfo->motor); + world->world->addMultiBodyConstraint(jointInfo->motor.get()); } if (jointInfo->motor) diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 79e4113aa..7a27f997c 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -284,14 +284,14 @@ void JointFeatures::SetJointVelocityCommand( if (!jointInfo->motor) { auto modelInfo = this->ReferenceInterface(jointInfo->model); - jointInfo->motor = new btMultiBodyJointMotor( + jointInfo->motor = std::make_shared( modelInfo->body.get(), std::get(jointInfo->identifier).indexInBtModel, 0, - 0, - jointInfo->effort); + static_cast(0), + static_cast(jointInfo->effort)); auto *world = this->ReferenceInterface(modelInfo->world); - world->world->addMultiBodyConstraint(jointInfo->motor); + world->world->addMultiBodyConstraint(jointInfo->motor.get()); } jointInfo->motor->setVelocityTarget(static_cast(_value)); diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 591f9a33f..5f63e5694 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -581,10 +581,12 @@ Identity SDFFeatures::ConstructSdfModel( model->body->getLink(i).m_jointMaxForce = static_cast(joint->Axis()->Effort()); jointInfo->effort = static_cast(joint->Axis()->Effort()); - btMultiBodyConstraint *con = new btMultiBodyJointLimitConstraint( + + jointInfo->jointLimits = + std::make_shared( model->body.get(), i, static_cast(joint->Axis()->Lower()), static_cast(joint->Axis()->Upper())); - world->world->addMultiBodyConstraint(con); + world->world->addMultiBodyConstraint(jointInfo->jointLimits.get()); } jointInfo->jointFeedback = std::make_shared(); @@ -884,21 +886,20 @@ bool SDFFeatures::AddSdfCollision( // NOTE: Bullet does not appear to support different surface properties // for different shapes attached to the same link. - auto collider = std::make_unique( + linkInfo->collider = std::make_unique( model->body.get(), linkIndexInModel); linkInfo->shape->addChildShape(btInertialToCollision, shape.get()); - collider->setCollisionShape(linkInfo->shape.get()); - collider->setRestitution(static_cast(restitution)); - collider->setRollingFriction(static_cast(rollingFriction)); - collider->setFriction(static_cast(mu)); - collider->setAnisotropicFriction( + linkInfo->collider->setCollisionShape(linkInfo->shape.get()); + linkInfo->collider->setRestitution(static_cast(restitution)); + linkInfo->collider->setRollingFriction( + static_cast(rollingFriction)); + linkInfo->collider->setFriction(static_cast(mu)); + linkInfo->collider->setAnisotropicFriction( btVector3(static_cast(mu), static_cast(mu2), 1), btCollisionObject::CF_ANISOTROPIC_FRICTION); - linkInfo->collider = std::move(collider); - if (linkIndexInModel >= 0) { model->body->getLink(linkIndexInModel).m_collider = diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh index 743a3e853..fdc6f6795 100644 --- a/bullet/src/Base.hh +++ b/bullet/src/Base.hh @@ -238,6 +238,26 @@ class Base : public Implements3d> return this->GenerateIdentity(id, this->joints.at(id)); } + public: ~Base() override + { + this->joints.clear(); + + for (const auto &[k, link] : links) + { + auto *model = this->ReferenceInterface(link->model); + auto *world = this->ReferenceInterface(model->world); + if (link->link != nullptr) + { + world->world->removeCollisionObject(link->link.get()); + } + } + + this->collisions.clear(); + this->links.clear(); + this->models.clear(); + this->worlds.clear(); + } + public: using WorldInfoPtr = std::shared_ptr; public: using ModelInfoPtr = std::shared_ptr; public: using LinkInfoPtr = std::shared_ptr; From 80aaef1eea1255205eb443430744fc9e54af29cf Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 20 Sep 2023 16:47:23 -0700 Subject: [PATCH 4/5] joint_features test: reduce console spam (#543) This sets console verbosity to zero when passing NaN commands in the test to reduce console spam. Signed-off-by: Steve Peters --- test/common_test/joint_features.cc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 00a4e2830..2ceb48b83 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -143,7 +143,10 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand) // Check that invalid velocity commands don't cause collisions to fail for (std::size_t i = 0; i < 1000; ++i) { + // Silence console spam + gz::common::Console::SetVerbosity(0); joint->SetForce(0, std::numeric_limits::quiet_NaN()); + gz::common::Console::SetVerbosity(4); // expect the position of the pendulum to stay above ground world->Step(output, state, input); auto frameData = base_link->FrameDataRelativeToWorld(); @@ -178,7 +181,10 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand) // Check that invalid velocity commands don't cause collisions to fail for (std::size_t i = 0; i < 1000; ++i) { + // Silence console spam + gz::common::Console::SetVerbosity(0); joint->SetVelocityCommand(0, std::numeric_limits::quiet_NaN()); + gz::common::Console::SetVerbosity(4); // expect the position of the pendulum to stay above ground world->Step(output, state, input); auto frameData = base_link->FrameDataRelativeToWorld(); From d1a76c00f3247ca11e87680d4a349ac68f4df52b Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 26 Sep 2023 17:06:50 -0500 Subject: [PATCH 5/5] Prepare for 6.5.1 release (#552) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 797bfe24a..01ffa1b22 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-physics6 VERSION 6.5.0) +project(gz-physics6 VERSION 6.5.1) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index 7801744a7..57c3b1118 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,13 @@ ## Gazebo Physics 6.x +### Gazebo Physics 6.5.1 (2023-09-26) + +1. joint_features test: reduce console spam + * [Pull request #543](https://github.com/gazebosim/gz-physics/pull/543) + +1. Cleaning up bullet memory use issues + * [Pull request #539](https://github.com/gazebosim/gz-physics/pull/539) + ### Gazebo Physics 6.5.0 (2023-08-30) 1. Add optional binary relocatability