Skip to content

Commit

Permalink
Replace raw pointers with shared pointers
Browse files Browse the repository at this point in the history 
Signed-off-by: Jorge Perez <jjperez@ekumenlabs.com>
  • Loading branch information
@Blast545
Blast545 committed Mar 11, 2021
1 parent 409c448 commit f7b1d0a
Show file tree
Hide file tree
Showing 6 changed files with 72 additions and 69 deletions.
18 changes: 10 additions & 8 deletions bullet/src/Base.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,12 +46,12 @@ namespace bullet {
// todo(anyone): Handle cleaning these pointers
struct WorldInfo
{
btDiscreteDynamicsWorld* world;
std::shared_ptr<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;
};

struct ModelInfo
Expand All @@ -66,17 +66,19 @@ struct ModelInfo
struct LinkInfo
{
std::string name;
btRigidBody* link;
std::shared_ptr<btRigidBody> link;
Identity model;
math::Pose3d pose;
double mass;
btVector3 inertia;
std::shared_ptr<btDefaultMotionState> motionState;
std::shared_ptr<btCompoundShape> btCollisionShape;
};

struct CollisionInfo
{
std::string name;
btCollisionShape* shape;
std::shared_ptr<btCollisionShape> shape;
Identity link;
Identity model;
math::Pose3d pose;
Expand All @@ -87,7 +89,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;
std::shared_ptr<btTypedConstraint> joint;
// links associated with this constraint, not sure if needed
std::size_t childLinkId;
std::size_t parentLinkId;
Expand Down
35 changes: 14 additions & 21 deletions bullet/src/EntityManagementFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,20 +32,20 @@ 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 if this line does something */
world->getSolverInfo().m_globalCfm = 0;

btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher.get());

return this->AddWorld(
{world, _name, collisionConfiguration, dispatcher, broadphase, solver});
Expand Down Expand Up @@ -73,9 +73,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 @@ -90,7 +88,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 @@ -105,8 +102,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 @@ -149,9 +145,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 @@ -166,7 +160,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 @@ -182,7 +175,7 @@ bool EntityManagementFeatures::RemoveModelByIndex(

if (linkInfo->model.id == _modelIndex)
{
bulletWorld->removeRigidBody(linkInfo->link);
bulletWorld->removeRigidBody(linkInfo->link.get());
it = this->links.erase(it);
continue;
}
Expand Down
22 changes: 11 additions & 11 deletions bullet/src/JointFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ double JointFeatures::GetJointPosition(
static_cast<int>(::sdf::JointType::REVOLUTE))
{
btHingeAccumulatedAngleConstraint* hinge =
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint);
static_cast<btHingeAccumulatedAngleConstraint*>(jointInfo->joint.get());
if (hinge)
{
result = hinge->getAccumulatedHingeAngle();
Expand Down Expand Up @@ -72,7 +72,7 @@ double JointFeatures::GetJointVelocity(
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 @@ -85,7 +85,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 @@ -95,7 +95,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 @@ -130,7 +130,7 @@ double JointFeatures::GetJointAcceleration(
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 @@ -143,7 +143,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 @@ -154,7 +154,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 @@ -190,7 +190,7 @@ double JointFeatures::GetJointForce(
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 @@ -203,15 +203,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 @@ -281,7 +281,7 @@ void JointFeatures::SetJointForce(
if (jointType == 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
45 changes: 26 additions & 19 deletions bullet/src/SDFFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,21 +108,28 @@ Identity SDFFeatures::ConstructSdfLink(
linkInertiaDiag = btVector3(0, 0, 0);
}

auto myMotionState = std::make_shared<btDefaultMotionState>(baseTransform);
auto collisionShape = std::make_shared<btCompoundShape>();
btRigidBody::btRigidBodyConstructionInfo
rbInfo(mass, myMotionState.get(), collisionShape.get(), linkInertiaDiag);
/*
btDefaultMotionState* myMotionState = new btDefaultMotionState(baseTransform);
btCollisionShape* collisionShape = new btCompoundShape();
btRigidBody::btRigidBodyConstructionInfo
rbInfo(mass, myMotionState, collisionShape, linkInertiaDiag);
btRigidBody* body = new btRigidBody(rbInfo);
body->setActivationState(DISABLE_DEACTIVATION);
rbInfo(mass, myMotionState, collisionShape, 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});
linkInertiaDiag, myMotionState, collisionShape});
return linkIdentity;
}

Expand All @@ -138,33 +145,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 @@ -202,7 +209,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 @@ -285,21 +292,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 @@ -312,8 +319,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);

double damping = 0;
Expand Down
Loading

0 comments on commit f7b1d0a

Please sign in to comment.