diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh index 14ac2c57e..a90e1ad14 100644 --- a/bullet/src/Base.hh +++ b/bullet/src/Base.hh @@ -43,15 +43,20 @@ namespace bullet { /// GenerateIdentity. /// 3) Hold explicit copies of raw pointers that can be deallocated -// todo(anyone): Handle cleaning these pointers +// TO-DO(): Consider using unique_ptrs instead of shared pointers +// for Bullet internal objects + +// Note: For Bullet library it's important the order in which the elements +// are destroyed. The current implementation relies on C++ destroying the +// elements in the opposite order stated in the structure struct WorldInfo { - btDiscreteDynamicsWorld* world; std::string name; - btDefaultCollisionConfiguration* collisionConfiguration; - btCollisionDispatcher* dispatcher; - btBroadphaseInterface* broadphase; - btConstraintSolver* solver; + std::shared_ptr collisionConfiguration; + std::shared_ptr dispatcher; + std::shared_ptr broadphase; + std::shared_ptr solver; + std::shared_ptr world; }; struct ModelInfo @@ -66,17 +71,19 @@ struct ModelInfo struct LinkInfo { std::string name; - btRigidBody* link; Identity model; math::Pose3d pose; double mass; btVector3 inertia; + std::shared_ptr motionState; + std::shared_ptr collisionShape; + std::shared_ptr link; }; struct CollisionInfo { std::string name; - btCollisionShape* shape; + std::shared_ptr shape; Identity link; Identity model; math::Pose3d pose; @@ -86,9 +93,7 @@ struct JointInfo { std::string name; // Base class for all the constraint objects, - // Not sure atm if it's possible to have it to manage all derived classes - btTypedConstraint* joint; - // links associated with this constraint, not sure if needed + std::shared_ptr joint; std::size_t childLinkId; std::size_t parentLinkId; int constraintType; diff --git a/bullet/src/EntityManagementFeatures.cc b/bullet/src/EntityManagementFeatures.cc index f405826d8..91cae4136 100644 --- a/bullet/src/EntityManagementFeatures.cc +++ b/bullet/src/EntityManagementFeatures.cc @@ -21,6 +21,7 @@ #include #include "EntityManagementFeatures.hh" +#include namespace ignition { namespace physics { @@ -31,20 +32,23 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( const Identity &/*_engineID*/, const std::string &_name) { // Create bullet empty multibody dynamics world - btDefaultCollisionConfiguration* collisionConfiguration = - new btDefaultCollisionConfiguration(); - btCollisionDispatcher* dispatcher = - new btCollisionDispatcher(collisionConfiguration); - btBroadphaseInterface* broadphase = new btDbvtBroadphase(); - - auto solver = new btSequentialImpulseConstraintSolver; - auto world = new btDiscreteDynamicsWorld( - dispatcher, broadphase, solver, collisionConfiguration); - + const auto collisionConfiguration = std::make_shared(); + const auto dispatcher = + std::make_shared(collisionConfiguration.get()); + //const auto broadphase = std::shared_ptr(new btDbvtBroadphase()); + const auto broadphase = std::make_shared(); + const auto solver = + std::make_shared(); + const auto world = std::make_shared( + dispatcher.get(), broadphase.get(), solver.get(), collisionConfiguration.get()); + + /* TO-DO(Lobotuerk): figure out what this line does*/ world->getSolverInfo().m_globalCfm = 0; + btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher.get()); + return this->AddWorld( - {world, _name, collisionConfiguration, dispatcher, broadphase, solver}); + {_name, collisionConfiguration, dispatcher, broadphase, solver, world}); } ///////////////////////////////////////////////// @@ -68,9 +72,7 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) const auto &childLinkInfo = this->links[jointInfo->childLinkId]; if (childLinkInfo->model.id == _modelID.id) { - btTypedConstraint* constraint = jointInfo->joint; - bulletWorld->removeConstraint(constraint); - delete constraint; + bulletWorld->removeConstraint(jointInfo->joint.get()); joint_it = this->joints.erase(joint_it); continue; } @@ -84,7 +86,6 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) const auto &collisionInfo = collision_it->second; if (collisionInfo->model.id == _modelID.id) { - delete collisionInfo->shape; collision_it = this->collisions.erase(collision_it); continue; } @@ -98,8 +99,7 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) const auto &linkInfo = it->second; if (linkInfo->model.id == _modelID.id) { - bulletWorld->removeRigidBody(linkInfo->link); - delete linkInfo->link; + bulletWorld->removeRigidBody(linkInfo->link.get()); it = this->links.erase(it); continue; } @@ -141,9 +141,7 @@ bool EntityManagementFeatures::RemoveModelByIndex( const auto &childLinkInfo = this->links[jointInfo->childLinkId]; if (childLinkInfo->model.id == _modelIndex) { - btTypedConstraint* constraint = jointInfo->joint; - bulletWorld->removeConstraint(constraint); - delete constraint; + bulletWorld->removeConstraint(jointInfo->joint.get()); joint_it = this->joints.erase(joint_it); continue; } @@ -157,7 +155,6 @@ bool EntityManagementFeatures::RemoveModelByIndex( const auto &collisionInfo = collision_it->second; if (collisionInfo->model.id == _modelIndex) { - delete collisionInfo->shape; collision_it = this->collisions.erase(collision_it); continue; } @@ -172,8 +169,7 @@ bool EntityManagementFeatures::RemoveModelByIndex( if (linkInfo->model.id == _modelIndex) { - bulletWorld->removeRigidBody(linkInfo->link); - delete linkInfo->link; + bulletWorld->removeRigidBody(linkInfo->link.get()); it = this->links.erase(it); continue; } diff --git a/bullet/src/JointFeatures.cc b/bullet/src/JointFeatures.cc index 2e78e398c..cd4320e24 100644 --- a/bullet/src/JointFeatures.cc +++ b/bullet/src/JointFeatures.cc @@ -39,7 +39,8 @@ double JointFeatures::GetJointPosition( { case static_cast(::sdf::JointType::REVOLUTE) : { - btHingeAccumulatedAngleConstraint* hinge = static_cast(jointInfo->joint); + btHingeAccumulatedAngleConstraint* hinge = + static_cast(jointInfo->joint.get()); if (hinge) { result = hinge->getAccumulatedHingeAngle(); @@ -74,7 +75,7 @@ double JointFeatures::GetJointVelocity( case static_cast(::sdf::JointType::REVOLUTE) : { btHingeAccumulatedAngleConstraint* hinge = - static_cast(jointInfo->joint); + static_cast(jointInfo->joint.get()); if (hinge) { result = 0.0; @@ -87,7 +88,7 @@ double JointFeatures::GetJointVelocity( if (this->links.find(jointInfo->childLinkId) != this->links.end()) { - btRigidBody *childLink = this->links.at(jointInfo->childLinkId)->link; + btRigidBody *childLink = this->links.at(jointInfo->childLinkId)->link.get(); btVector3 aux = childLink->getAngularVelocity(); math::Vector3 angularVelocity(aux[0], aux[1], aux[2]); // result += @@ -97,7 +98,7 @@ double JointFeatures::GetJointVelocity( if (this->links.find(jointInfo->parentLinkId) != this->links.end()) { btRigidBody *parentLink = - this->links.at(jointInfo->parentLinkId)->link; + this->links.at(jointInfo->parentLinkId)->link.get(); btVector3 aux = parentLink->getAngularVelocity(); math::Vector3 angularVelocity(aux[0], aux[1], aux[2]); // result -= @@ -134,7 +135,7 @@ double JointFeatures::GetJointAcceleration( case static_cast(::sdf::JointType::REVOLUTE) : { btHingeAccumulatedAngleConstraint* hinge = - static_cast(jointInfo->joint); + static_cast(jointInfo->joint.get()); if (hinge) { result = 0.0; @@ -147,7 +148,7 @@ double JointFeatures::GetJointAcceleration( if (this->links.find(jointInfo->childLinkId) != this->links.end()) { - btRigidBody *childLink = this->links.at(jointInfo->childLinkId)->link; + btRigidBody *childLink = this->links.at(jointInfo->childLinkId)->link.get(); btVector3 aux = childLink->getTotalTorque(); math::Vector3 angularTorque(aux[0], aux[1], aux[2]); const btVector3 localInertia = childLink->getLocalInertia(); @@ -158,7 +159,7 @@ double JointFeatures::GetJointAcceleration( if (this->links.find(jointInfo->parentLinkId) != this->links.end()) { btRigidBody *parentLink = - this->links.at(jointInfo->parentLinkId)->link; + this->links.at(jointInfo->parentLinkId)->link.get(); btVector3 aux = parentLink->getTotalTorque(); math::Vector3 angularTorque(aux[0], aux[1], aux[2]); const btVector3 localInertia = parentLink->getLocalInertia(); @@ -196,7 +197,7 @@ double JointFeatures::GetJointForce( case static_cast(::sdf::JointType::REVOLUTE) : { btHingeAccumulatedAngleConstraint* hinge = - static_cast(jointInfo->joint); + static_cast(jointInfo->joint.get()); if (hinge) { result = 0.0; @@ -209,7 +210,7 @@ double JointFeatures::GetJointForce( if (this->links.find(jointInfo->childLinkId) != this->links.end()) { - btRigidBody *childLink = this->links.at(jointInfo->childLinkId)->link; + btRigidBody *childLink = this->links.at(jointInfo->childLinkId)->link.get(); btVector3 aux = childLink->getTotalTorque(); math::Vector3 angularTorque(aux[0], aux[1], aux[2]); result += globalAxis.Dot(angularTorque); @@ -217,7 +218,7 @@ double JointFeatures::GetJointForce( if (this->links.find(jointInfo->parentLinkId) != this->links.end()) { btRigidBody *parentLink = - this->links.at(jointInfo->parentLinkId)->link; + this->links.at(jointInfo->parentLinkId)->link.get(); btVector3 aux = parentLink->getTotalTorque(); math::Vector3 angularTorque(aux[0], aux[1], aux[2]); result -= globalAxis.Dot(angularTorque); @@ -290,7 +291,7 @@ void JointFeatures::SetJointForce( case static_cast(::sdf::JointType::REVOLUTE) : { btHingeAccumulatedAngleConstraint* hinge = - static_cast(jointInfo->joint); + static_cast(jointInfo->joint.get()); if (hinge) { // Limit the max torque applied to avoid abrupt changes in the diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index 83aa965a6..c814955e5 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -107,21 +107,23 @@ Identity SDFFeatures::ConstructSdfLink( linkInertiaDiag = btVector3(0, 0, 0); } - btDefaultMotionState* myMotionState = new btDefaultMotionState(baseTransform); - btCollisionShape* collisionShape = new btCompoundShape(); + auto myMotionState = std::make_shared(baseTransform); + auto collisionShape = std::make_shared(); btRigidBody::btRigidBodyConstructionInfo - rbInfo(mass, myMotionState, collisionShape, linkInertiaDiag); - btRigidBody* body = new btRigidBody(rbInfo); - body->setActivationState(DISABLE_DEACTIVATION); + rbInfo(mass, myMotionState.get(), collisionShape.get(), linkInertiaDiag); + + auto body = std::make_shared(rbInfo); + body.get()->setActivationState(DISABLE_DEACTIVATION); const auto &world = this->worlds.at(modelInfo->world)->world; // Links collide with everything except elements sharing a joint - world->addRigidBody(body); + world->addRigidBody(body.get()); // Generate an identity for it - const auto linkIdentity = this->AddLink({name, body, _modelID, pose, mass, - linkInertiaDiag}); + const auto linkIdentity = this->AddLink({name, _modelID, pose, mass, + linkInertiaDiag, myMotionState, collisionShape, body}); + return linkIdentity; } @@ -137,33 +139,33 @@ Identity SDFFeatures::ConstructSdfCollision( } const auto &geom = _collision.Geom(); - btCollisionShape* shape = nullptr; + std::shared_ptr shape; if (geom->BoxShape()) { const auto box = geom->BoxShape(); const auto size = math::eigen3::convert(box->Size()); const auto halfExtents = convertVec(size)*0.5; - shape = new btBoxShape(halfExtents); + shape = std::make_shared(halfExtents); } else if (geom->SphereShape()) { const auto sphere = geom->SphereShape(); const auto radius = sphere->Radius(); - shape = new btSphereShape(radius); + shape = std::make_shared(radius); } else if (geom->CylinderShape()) { const auto cylinder = geom->CylinderShape(); const auto radius = cylinder->Radius(); const auto halfLength = cylinder->Length()*0.5; - shape = new btCylinderShapeZ(btVector3(radius, radius, halfLength)); + shape = std::make_shared(btVector3(radius, radius, halfLength)); } else if (geom->PlaneShape()) { const auto plane = geom->PlaneShape(); const auto normal = convertVec(math::eigen3::convert(plane->Normal())); - shape = new btStaticPlaneShape(normal, 0); + shape = std::make_shared(normal, 0); } // TODO(lobotuerk/blast545) Add additional friction parameters for bullet @@ -201,7 +203,7 @@ Identity SDFFeatures::ConstructSdfCollision( btCollisionObject::CF_ANISOTROPIC_FRICTION); dynamic_cast(body->getCollisionShape()) - ->addChildShape(baseTransform, shape); + ->addChildShape(baseTransform, shape.get()); auto identity = this->AddCollision({_collision.Name(), shape, _linkID, modelID, pose}); @@ -284,12 +286,12 @@ Identity SDFFeatures::ConstructSdfJoint( axisChild = pose.Rot().RotateVectorReverse(axis); axisChild = axisChild.Normalize(); - btHingeAccumulatedAngleConstraint* joint; + std::shared_ptr joint; if (parentId != worldId) { - joint = new btHingeAccumulatedAngleConstraint( - *this->links.at(childId)->link, - *this->links.at(parentId)->link, + joint = std::make_shared( + *this->links.at(childId)->link.get(), + *this->links.at(parentId)->link.get(), convertVec(ignition::math::eigen3::convert(pivotChild)), convertVec(ignition::math::eigen3::convert(pivotParent)), convertVec(ignition::math::eigen3::convert(axisChild)), @@ -297,8 +299,8 @@ Identity SDFFeatures::ConstructSdfJoint( } else { - joint = new btHingeAccumulatedAngleConstraint( - *this->links.at(childId)->link, + joint = std::make_shared( + *this->links.at(childId)->link.get(), convertVec(ignition::math::eigen3::convert(pivotChild)), convertVec(ignition::math::eigen3::convert(axisChild))); } @@ -311,8 +313,8 @@ Identity SDFFeatures::ConstructSdfJoint( } const auto &modelInfo = this->models.at(_modelID); - const auto &world = this->worlds.at(modelInfo->world)->world; - world->addConstraint(joint, true); + const auto &world = this->worlds.at(modelInfo->world)->world.get(); + world->addConstraint(joint.get(), true); joint->enableFeedback(true); /* TO-DO(Lobotuerk): find how to implement axis friction properly for bullet*/ diff --git a/bullet/src/ShapeFeatures.cc b/bullet/src/ShapeFeatures.cc index 29e778c8a..ce1ecddd8 100644 --- a/bullet/src/ShapeFeatures.cc +++ b/bullet/src/ShapeFeatures.cc @@ -17,7 +17,6 @@ #include "ShapeFeatures.hh" #include -#include namespace ignition { namespace physics { @@ -38,7 +37,7 @@ Identity ShapeFeatures::AttachMeshShape( unsigned int numVertices = _mesh.VertexCount(); unsigned int numIndices = _mesh.IndexCount(); - btTriangleMesh *mTriMesh = new btTriangleMesh(); + const auto mTriMesh = std::make_shared(new btTriangleMesh()); for (unsigned int j = 0; j < numVertices; ++j) { @@ -63,12 +62,12 @@ Identity ShapeFeatures::AttachMeshShape( vertices[indices[j+2]*3+1], vertices[indices[j+2]*3+2]); - mTriMesh->addTriangle(bv0, bv1, bv2); + mTriMesh.get()->addTriangle(bv0, bv1, bv2); } - btGImpactMeshShape *gimpactMeshShape = - new btGImpactMeshShape(mTriMesh); - gimpactMeshShape->updateBound(); + auto gimpactMeshShape = + std::make_shared(mTriMesh.get()); + gimpactMeshShape.get()->updateBound(); // TODO(lobotuerk) Save collision if needed // collision->shape = gimpactMeshShape; @@ -78,9 +77,7 @@ Identity ShapeFeatures::AttachMeshShape( const auto &linkInfo = this->links.at(_linkID); const auto &modelID = linkInfo->model; - const auto &modelInfo = this->models.at(modelID); - const auto &worldInfo = this->worlds.at(modelInfo->world); - const auto &body = linkInfo->link; + const auto &body = linkInfo->link.get(); const auto poseIsometry = _pose; const auto poseTranslation = poseIsometry.translation(); @@ -89,11 +86,11 @@ Identity ShapeFeatures::AttachMeshShape( baseTransform.setOrigin(convertVec(poseTranslation)); baseTransform.setBasis(convertMat(poseLinear)); + /* TO-DO(Lobotuerk): figure out if this line is needed */ // gimpactMeshShape->setMargin(btScalar(0.001)); dynamic_cast( - body->getCollisionShape())->addChildShape(baseTransform, gimpactMeshShape); - btGImpactCollisionAlgorithm::registerAlgorithm(worldInfo->dispatcher); + body->getCollisionShape())->addChildShape(baseTransform, gimpactMeshShape.get()); auto identity = this->AddCollision( {_name, gimpactMeshShape, _linkID, modelID,