diff --git a/Changelog.md b/Changelog.md index 43d91ea60..82936992e 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,14 @@ ## 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 @@ -228,6 +236,18 @@ ## Gazebo Physics 5.x +### Gazebo 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) + ### Gazebo Physics 5.3.1 (2023-02-16) 1. Fix memory corruption due to faulty refcount tracking diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 9783ba22e..a45c4fa6e 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -166,9 +167,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 gearConstraint = nullptr; std::shared_ptr jointFeedback = nullptr; @@ -357,6 +359,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; @@ -369,8 +395,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 d0b0d8fb8..2cc1286f9 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -285,14 +285,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; 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();