Skip to content

Commit

Permalink
Replace raw pointers with shared pointers
Browse files Browse the repository at this point in the history
  • Loading branch information
@Blast545
Blast545 committed Mar 12, 2021
1 parent 607b609 commit bd154bb
Show file tree
Hide file tree
Showing 5 changed files with 79 additions and 78 deletions.
27 changes: 16 additions & 11 deletions bullet/src/Base.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<btDefaultCollisionConfiguration> collisionConfiguration;
std::shared_ptr<btCollisionDispatcher> dispatcher;
std::shared_ptr<btBroadphaseInterface> broadphase;
std::shared_ptr<btConstraintSolver> solver;
std::shared_ptr<btDiscreteDynamicsWorld> world;
};

struct ModelInfo
Expand All @@ -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<btDefaultMotionState> motionState;
std::shared_ptr<btCompoundShape> collisionShape;
std::shared_ptr<btRigidBody> link;
};

struct CollisionInfo
{
std::string name;
btCollisionShape* shape;
std::shared_ptr<btCollisionShape> shape;
Identity link;
Identity model;
math::Pose3d pose;
Expand All @@ -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<btTypedConstraint> joint;
std::size_t childLinkId;
std::size_t parentLinkId;
int constraintType;
Expand Down
42 changes: 19 additions & 23 deletions bullet/src/EntityManagementFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <unordered_map>

#include "EntityManagementFeatures.hh"
#include <BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h>

namespace ignition {
namespace physics {
Expand All @@ -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<btDefaultCollisionConfiguration>();
const auto dispatcher =
std::make_shared<btCollisionDispatcher>(collisionConfiguration.get());
//const auto broadphase = std::shared_ptr<btBroadphaseInterface>(new btDbvtBroadphase());
const auto broadphase = std::make_shared<btDbvtBroadphase>();
const auto solver =
std::make_shared<btSequentialImpulseConstraintSolver>();
const auto world = std::make_shared<btDiscreteDynamicsWorld>(
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});
}

/////////////////////////////////////////////////
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand Down
23 changes: 12 additions & 11 deletions bullet/src/JointFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,8 @@ double JointFeatures::GetJointPosition(
{
case static_cast<int>(::sdf::JointType::REVOLUTE) :
{
btHingeAccumulatedAngleConstraint* hinge = static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint);
btHingeAccumulatedAngleConstraint* hinge =
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint.get());
if (hinge)
{
result = hinge->getAccumulatedHingeAngle();
Expand Down Expand Up @@ -74,7 +75,7 @@ double JointFeatures::GetJointVelocity(
case static_cast<int>(::sdf::JointType::REVOLUTE) :
{
btHingeAccumulatedAngleConstraint* hinge =
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint);
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint.get());
if (hinge)
{
result = 0.0;
Expand All @@ -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 +=
Expand All @@ -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 -=
Expand Down Expand Up @@ -134,7 +135,7 @@ double JointFeatures::GetJointAcceleration(
case static_cast<int>(::sdf::JointType::REVOLUTE) :
{
btHingeAccumulatedAngleConstraint* hinge =
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint);
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint.get());
if (hinge)
{
result = 0.0;
Expand All @@ -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();
Expand All @@ -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();
Expand Down Expand Up @@ -196,7 +197,7 @@ double JointFeatures::GetJointForce(
case static_cast<int>(::sdf::JointType::REVOLUTE) :
{
btHingeAccumulatedAngleConstraint* hinge =
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint);
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint.get());
if (hinge)
{
result = 0.0;
Expand All @@ -209,15 +210,15 @@ 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);
}
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);
Expand Down Expand Up @@ -290,7 +291,7 @@ void JointFeatures::SetJointForce(
case static_cast<int>(::sdf::JointType::REVOLUTE) :
{
btHingeAccumulatedAngleConstraint* hinge =
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint);
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint.get());
if (hinge)
{
// Limit the max torque applied to avoid abrupt changes in the
Expand Down
46 changes: 24 additions & 22 deletions bullet/src/SDFFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<btDefaultMotionState>(baseTransform);
auto collisionShape = std::make_shared<btCompoundShape>();
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<btRigidBody>(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;
}

Expand All @@ -137,33 +139,33 @@ Identity SDFFeatures::ConstructSdfCollision(
}

const auto &geom = _collision.Geom();
btCollisionShape* shape = nullptr;
std::shared_ptr<btCollisionShape> 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<btBoxShape>(halfExtents);
}
else if (geom->SphereShape())
{
const auto sphere = geom->SphereShape();
const auto radius = sphere->Radius();
shape = new btSphereShape(radius);
shape = std::make_shared<btSphereShape>(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<btCylinderShapeZ>(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<btStaticPlaneShape>(normal, 0);
}

// TODO(lobotuerk/blast545) Add additional friction parameters for bullet
Expand Down Expand Up @@ -201,7 +203,7 @@ Identity SDFFeatures::ConstructSdfCollision(
btCollisionObject::CF_ANISOTROPIC_FRICTION);

dynamic_cast<btCompoundShape *>(body->getCollisionShape())
->addChildShape(baseTransform, shape);
->addChildShape(baseTransform, shape.get());

auto identity = this->AddCollision({_collision.Name(), shape, _linkID,
modelID, pose});
Expand Down Expand Up @@ -284,21 +286,21 @@ Identity SDFFeatures::ConstructSdfJoint(
axisChild = pose.Rot().RotateVectorReverse(axis);
axisChild = axisChild.Normalize();

btHingeAccumulatedAngleConstraint* joint;
std::shared_ptr<btHingeAccumulatedAngleConstraint> joint;
if (parentId != worldId)
{
joint = new btHingeAccumulatedAngleConstraint(
*this->links.at(childId)->link,
*this->links.at(parentId)->link,
joint = std::make_shared<btHingeAccumulatedAngleConstraint>(
*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)),
convertVec(ignition::math::eigen3::convert(axisParent)));
}
else
{
joint = new btHingeAccumulatedAngleConstraint(
*this->links.at(childId)->link,
joint = std::make_shared<btHingeAccumulatedAngleConstraint>(
*this->links.at(childId)->link.get(),
convertVec(ignition::math::eigen3::convert(pivotChild)),
convertVec(ignition::math::eigen3::convert(axisChild)));
}
Expand All @@ -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*/
Expand Down
Loading

0 comments on commit bd154bb

Please sign in to comment.