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;