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 <[email protected]> Signed-off-by: Addisu Z. Taddese <[email protected]> Co-authored-by: Addisu Z. Taddese <[email protected]>
gazebosim · Sep 20, 2023 · 6a21a96 · 6a21a96
1 parent f466300
commit 6a21a96
Show file tree

Hide file tree

Showing 5 changed files with 70 additions and 23 deletions.
diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh
@@ -26,6 +26,7 @@
 #include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
 #include <BulletDynamics/Featherstone/btMultiBodyJointFeedback.h>
 #include <BulletDynamics/Featherstone/btMultiBodyJointMotor.h>
+#include <BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h>
 #include <BulletDynamics/Featherstone/btMultiBodyLinkCollider.h>
 #include <BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h>
 #include <LinearMath/btVector3.h>
@@ -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<btMultiBodyJointMotor> motor = nullptr;
+  std::shared_ptr<btMultiBodyJointLimitConstraint> jointLimits = nullptr;
   std::shared_ptr<btMultiBodyFixedConstraint> fixedConstraint = nullptr;
   std::shared_ptr<btMultiBodyJointFeedback> jointFeedback = nullptr;
 };
@@ -355,6 +357,30 @@ class Base : public Implements3d<FeatureList<Feature>>
     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<ModelInfo>(link->model);
+        auto *world = this->ReferenceInterface<WorldInfo>(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<WorldInfo>;
   public: using ModelInfoPtr = std::shared_ptr<ModelInfo>;
   public: using LinkInfoPtr  = std::shared_ptr<LinkInfo>;
@@ -367,8 +393,6 @@ class Base : public Implements3d<FeatureList<Feature>>
   public: std::unordered_map<std::size_t, CollisionInfoPtr> collisions;
   public: std::unordered_map<std::size_t, JointInfoPtr> joints;
 
-  // Note, the order of triangleMeshes and meshesGImpact is important. Reversing
-  // the order causes segfaults on macOS during destruction.
   public: std::vector<std::unique_ptr<btTriangleMesh>> triangleMeshes;
   public: std::vector<std::unique_ptr<btGImpactMeshShape>> meshesGImpact;
 };

diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc
@@ -17,6 +17,8 @@
 
 #include "FreeGroupFeatures.hh"
 
+#include <memory>
+
 namespace gz {
 namespace physics {
 namespace bullet_featherstone {
@@ -64,23 +66,23 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
   // Free groups in bullet-featherstone are always represented by ModelInfo
   const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
 
-  if(model)
+  if (model != nullptr)
   {
     // Set angular velocity the each one of the joints of the model
     for (const auto& jointID : model->jointEntityIds)
     {
       auto jointInfo = this->joints[jointID];
       if (!jointInfo->motor)
       {
-        auto modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
-        jointInfo->motor = new btMultiBodyJointMotor(
+        auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
+        jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
           modelInfo->body.get(),
           std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
           0,
-          0,
-          jointInfo->effort);
+          static_cast<btScalar>(0),
+          static_cast<btScalar>(jointInfo->effort));
         auto *world = this->ReferenceInterface<WorldInfo>(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
@@ -284,14 +284,14 @@ void JointFeatures::SetJointVelocityCommand(
   if (!jointInfo->motor)
   {
     auto modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
-    jointInfo->motor = new btMultiBodyJointMotor(
+    jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
       modelInfo->body.get(),
       std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
       0,
-      0,
-      jointInfo->effort);
+      static_cast<btScalar>(0),
+      static_cast<btScalar>(jointInfo->effort));
     auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
-    world->world->addMultiBodyConstraint(jointInfo->motor);
+    world->world->addMultiBodyConstraint(jointInfo->motor.get());
   }
 
   jointInfo->motor->setVelocityTarget(static_cast<btScalar>(_value));

diff --git 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<btScalar>(joint->Axis()->Effort());
         jointInfo->effort = static_cast<btScalar>(joint->Axis()->Effort());
-        btMultiBodyConstraint *con = new btMultiBodyJointLimitConstraint(
+
+        jointInfo->jointLimits =
+          std::make_shared<btMultiBodyJointLimitConstraint>(
             model->body.get(), i, static_cast<btScalar>(joint->Axis()->Lower()),
             static_cast<btScalar>(joint->Axis()->Upper()));
-        world->world->addMultiBodyConstraint(con);
+        world->world->addMultiBodyConstraint(jointInfo->jointLimits.get());
       }
 
       jointInfo->jointFeedback = std::make_shared<btMultiBodyJointFeedback>();
@@ -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<btMultiBodyLinkCollider>(
+      linkInfo->collider = std::make_unique<btMultiBodyLinkCollider>(
         model->body.get(), linkIndexInModel);
 
       linkInfo->shape->addChildShape(btInertialToCollision, shape.get());
 
-      collider->setCollisionShape(linkInfo->shape.get());
-      collider->setRestitution(static_cast<btScalar>(restitution));
-      collider->setRollingFriction(static_cast<btScalar>(rollingFriction));
-      collider->setFriction(static_cast<btScalar>(mu));
-      collider->setAnisotropicFriction(
+      linkInfo->collider->setCollisionShape(linkInfo->shape.get());
+      linkInfo->collider->setRestitution(static_cast<btScalar>(restitution));
+      linkInfo->collider->setRollingFriction(
+        static_cast<btScalar>(rollingFriction));
+      linkInfo->collider->setFriction(static_cast<btScalar>(mu));
+      linkInfo->collider->setAnisotropicFriction(
         btVector3(static_cast<btScalar>(mu), static_cast<btScalar>(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
@@ -238,6 +238,26 @@ class Base : public Implements3d<FeatureList<Feature>>
     return this->GenerateIdentity(id, this->joints.at(id));
   }
 
+  public: ~Base() override
+  {
+    this->joints.clear();
+
+    for (const auto &[k, link] : links)
+    {
+        auto *model = this->ReferenceInterface<ModelInfo>(link->model);
+        auto *world = this->ReferenceInterface<WorldInfo>(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<WorldInfo>;
   public: using ModelInfoPtr = std::shared_ptr<ModelInfo>;
   public: using LinkInfoPtr  = std::shared_ptr<LinkInfo>;