Merge: 6 -> 7 (#554)

Signed-off-by: Michael Carroll <[email protected]>

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

bullet-featherstone/src/Base.hh

@@ -27,6 +27,7 @@
 #include <BulletDynamics/Featherstone/btMultiBodyGearConstraint.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>
@@ -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<btMultiBodyJointMotor> motor = nullptr;
+  std::shared_ptr<btMultiBodyJointLimitConstraint> jointLimits = nullptr;
   std::shared_ptr<btMultiBodyFixedConstraint> fixedConstraint = nullptr;
   std::shared_ptr<btMultiBodyGearConstraint> gearConstraint = nullptr;
   std::shared_ptr<btMultiBodyJointFeedback> jointFeedback = nullptr;
@@ -357,6 +359,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>;
@@ -369,8 +395,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;
 };

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)

bullet-featherstone/src/JointFeatures.cc

@@ -285,14 +285,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));

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 =

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>;

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<double>::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<double>::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();