diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 0385c6e31..58a82d9c6 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -38,8 +38,10 @@ struct BulletURDFInternalData struct GUIHelperInterface* m_guiHelper; char m_pathPrefix[1024]; btHashMap m_linkColors; + btAlignedObjectArray m_allocatedCollisionShapes; }; + void BulletURDFImporter::printTree() { // btAssert(0); @@ -507,7 +509,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref } //if we have a convex, tesselate into localVertices/localIndices - if (convexColShape) + if ((glmesh==0) && convexColShape) { btShapeHull* hull = new btShapeHull(convexColShape); hull->buildHull(0.0); @@ -549,6 +551,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref glmesh->m_numvertices = glmesh->m_vertices->size(); glmesh->m_numIndices = glmesh->m_indices->size(); } + delete hull; delete convexColShape; convexColShape = 0; @@ -582,6 +585,8 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref verticesOut.push_back(v); } } + delete glmesh; + } @@ -811,12 +816,13 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co b3Warning("issue extracting mesh from STL file %s\n", fullPath); } + delete glmesh; + } else { b3Warning("mesh geometry not found %s\n",fullPath); } - } } @@ -888,10 +894,23 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP } +int BulletURDFImporter::getNumAllocatedCollisionShapes() const +{ + return m_data->m_allocatedCollisionShapes.size(); +} + + +btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index) +{ + return m_data->m_allocatedCollisionShapes[index]; +} + class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const { btCompoundShape* compoundShape = new btCompoundShape(); + m_data->m_allocatedCollisionShapes.push_back(compoundShape); + compoundShape->setMargin(0.001); #if USE_ROS_URDF_PARSER for (int v=0;v<(int)m_data->m_links[linkIndex]->collision_array.size();v++) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 1c148c729..1f9972511 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -38,7 +38,12 @@ public: virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; + ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation + virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; + + virtual int getNumAllocatedCollisionShapes() const; + virtual class btCollisionShape* getAllocatedCollisionShape(int index); }; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index f15392929..81df76996 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -9,7 +9,37 @@ UrdfParser::UrdfParser() } UrdfParser::~UrdfParser() { - //todo(erwincoumans) delete memory + + for (int i=0;i* m_indices; int m_numIndices; float m_scaling[4]; + + GLInstanceGraphicsShape() + :m_vertices(0), + m_indices(0) + { + + } + + virtual ~GLInstanceGraphicsShape() + { + delete m_vertices; + delete m_indices; + } }; #endif //GL_INSTANCE_GRAPHICS_SHAPE_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 62cdeea6e..bfd4705c6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -565,6 +565,9 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() } m_data->m_strings.clear(); + btAlignedObjectArray constraints; + btAlignedObjectArray mbconstraints; + if (m_data->m_dynamicsWorld) { @@ -572,8 +575,17 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() int i; for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--) { - m_data->m_dynamicsWorld->removeConstraint(m_data->m_dynamicsWorld->getConstraint(i)); + btTypedConstraint* constraint =m_data->m_dynamicsWorld->getConstraint(i); + constraints.push_back(constraint); + m_data->m_dynamicsWorld->removeConstraint(constraint); } + for (i=m_data->m_dynamicsWorld->getNumMultiBodyConstraints()-1;i>=0;i--) + { + btMultiBodyConstraint* mbconstraint = m_data->m_dynamicsWorld->getMultiBodyConstraint(i); + mbconstraints.push_back(mbconstraint); + m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint); + } + for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) { btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i]; @@ -585,8 +597,25 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() m_data->m_dynamicsWorld->removeCollisionObject(obj); delete obj; } + for (i=m_data->m_dynamicsWorld->getNumMultibodies()-1;i>=0;i--) + { + btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i); + m_data->m_dynamicsWorld->removeMultiBody(mb); + delete mb; + } } - //delete collision shapes + + for (int i=0;im_collisionShapes.size(); j++) { btCollisionShape* shape = m_data->m_collisionShapes[j]; @@ -665,6 +694,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto bool loadOk = u2b.loadURDF(fileName, useFixedBase); + + if (loadOk) { //get a body index @@ -695,9 +726,29 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto MyMultiBodyCreator creation(m_data->m_guiHelper); ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); + + + ///todo(erwincoumans) refactor this memory allocation issue + for (int i=0;im_collisionShapes.push_back(shape); + if (shape->isCompound()) + { + btCompoundShape* compound = (btCompoundShape*) shape; + for (int childIndex=0;childIndexgetNumChildShapes();childIndex++) + { + m_data->m_collisionShapes.push_back(compound->getChildShape(childIndex)); + } + } + + } + btMultiBody* mb = creation.getBulletMultiBody(); if (useMultiBody) { + + if (mb) { bodyHandle->m_multiBody = mb; @@ -743,7 +794,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); - return true; + return true; } else { b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); @@ -943,6 +994,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; + serverStatusOut.m_dataStreamArguments.m_streamChunkLength = 0; + if (m_data->m_urdfLinkNameMapper.size()) { serverStatusOut.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();