diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 3c5329809..ce49cf6f5 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -195,6 +195,14 @@ struct BulletMJCFImporterInternalData m_pathPrefix[0] = 0; } + ~BulletMJCFImporterInternalData() + { + for (int i=0;iAttribute("type"); const char* sz = link_xml->Attribute("size"); - const char* posS = link_xml->Attribute("pos"); int conDim = defaults.m_defaultConDim; const char* conDimS = link_xml->Attribute("condim"); @@ -656,7 +660,7 @@ struct BulletMJCFImporterInternalData btArray frictions; btAlignedObjectArray strArray; urdfIsAnyOf(" ", strArray); - urdfStringSplit(pieces, sz, strArray); + urdfStringSplit(pieces, frictionS, strArray); for (int i = 0; i < pieces.size(); ++i) { if (!pieces[i].empty()) @@ -707,6 +711,7 @@ struct BulletMJCFImporterInternalData geom.m_localMaterial.m_name = rgba; } + const char* posS = link_xml->Attribute("pos"); if (posS) { btVector3 pos(0,0,0); @@ -716,18 +721,32 @@ struct BulletMJCFImporterInternalData linkLocalFrame.setOrigin(pos); } } + const char* ornS = link_xml->Attribute("quat"); if (ornS) { - std::string ornStr = ornS; btQuaternion orn(0,0,0,1); btVector4 o4; - if (parseVector4(o4,ornStr)) + if (parseVector4(o4, ornS)) { orn.setValue(o4[1],o4[2],o4[3],o4[0]); linkLocalFrame.setRotation(orn); } } + + const char* axis_and_angle = link_xml->Attribute("axisangle"); + if (axis_and_angle) + { + btQuaternion orn(0,0,0,1); + btVector4 o4; + if (parseVector4(o4, axis_and_angle)) + { + orn.setRotation(btVector3(o4[0],o4[1],o4[2]), o4[3]); + linkLocalFrame.setRotation(orn); + } + } + + const char* gType = link_xml->Attribute("type"); if (gType) { std::string geomType = gType; @@ -792,8 +811,8 @@ struct BulletMJCFImporterInternalData } } - geom.m_capsuleRadius = 0; - geom.m_capsuleHeight = 0.f; + geom.m_capsuleRadius = 2.00f; // 2 to make it visible if something is wrong + geom.m_capsuleHeight = 2.00f; if (sizes.size()>0) { @@ -877,6 +896,7 @@ struct BulletMJCFImporterInternalData col.m_geometry = geom; col.m_linkLocalFrame = linkLocalFrame; + col.m_sourceFileLocation = sourceFileLocation(link_xml); linkPtr->m_collisionArray.push_back(col); } else @@ -950,22 +970,6 @@ struct BulletMJCFImporterInternalData col->m_geometry.m_boxSize[2]; break; } - - case URDF_GEOM_CYLINDER: - { - double r = col->m_geometry.m_capsuleRadius; - btScalar h(0); - //and one cylinder of 'height' - if (col->m_geometry.m_hasFromTo) - { - h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length(); - } else - { - h = col->m_geometry.m_capsuleHeight; - } - totalVolume += SIMD_PI*r*r*h; - break; - } case URDF_GEOM_MESH: { //todo (based on mesh bounding box?) @@ -976,11 +980,15 @@ struct BulletMJCFImporterInternalData //todo break; } + case URDF_GEOM_CYLINDER: case URDF_GEOM_CAPSULE: { //one sphere double r = col->m_geometry.m_capsuleRadius; - totalVolume += 4./3.*SIMD_PI*r*r*r; + if (col->m_geometry.m_type==URDF_GEOM_CAPSULE) + { + totalVolume += 4./3.*SIMD_PI*r*r*r; + } btScalar h(0); if (col->m_geometry.m_hasFromTo) { @@ -1017,9 +1025,9 @@ struct BulletMJCFImporterInternalData UrdfModel* modelPtr = m_models[modelIndex]; int orgChildLinkIndex = modelPtr->m_links.size(); UrdfLink* linkPtr = new UrdfLink(); - char uniqueLinkName[1024]; - sprintf(uniqueLinkName,"link%d",orgChildLinkIndex ); - linkPtr->m_name = uniqueLinkName; + char linkn[1024]; + sprintf(linkn, "link%d_%d", modelIndex, orgChildLinkIndex); + linkPtr->m_name = linkn; if (namePtr) { linkPtr->m_name = namePtr; @@ -1205,6 +1213,7 @@ struct BulletMJCFImporterInternalData } linkPtr->m_linkTransformInWorld = linkTransform; + if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint) { //linkPtr->m_linkTransformInWorld.setIdentity(); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 94dcfcb04..fcee5a150 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -285,6 +285,9 @@ void ConvertURDF2BulletInternal( if (!(flags & CUF_USE_URDF_INERTIA)) { compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); + btAssert(localInertiaDiagonal[0] < 1e10); + btAssert(localInertiaDiagonal[1] < 1e10); + btAssert(localInertiaDiagonal[2] < 1e10); } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index f5462ceff..74ff541f6 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -13,48 +13,10 @@ m_activeSdfModel(-1) UrdfParser::~UrdfParser() { - cleanModel(&m_urdf2Model); - - for (int i=0;im_materials.size();i++) - { - UrdfMaterial** matPtr = model->m_materials.getAtIndex(i); - if (matPtr) - { - UrdfMaterial* mat = *matPtr; - delete mat; - } - } - - for (int i=0;im_links.size();i++) - { - UrdfLink** linkPtr = model->m_links.getAtIndex(i); - if (linkPtr) - { - UrdfLink* link = *linkPtr; - delete link; - } - } - - for (int i=0;im_joints.size();i++) - { - UrdfJoint** jointPtr = model->m_joints.getAtIndex(i); - if (jointPtr) - { - UrdfJoint* joint = *jointPtr; - delete joint; - } - } + } } static bool parseVector4(btVector4& vec4, const std::string& vector_str) @@ -1471,6 +1433,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF UrdfMaterial** mat =m_urdf2Model.m_materials.find(material->m_name.c_str()); if (mat) { + delete material; logger->reportWarning("Duplicate material"); } else { @@ -1494,6 +1457,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF { logger->reportError("Link name is not unique, link names in the same model have to be unique"); logger->reportError(link->m_name.c_str()); + delete link; return false; } else { @@ -1542,6 +1506,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF { logger->reportError("joint '%s' is not unique."); logger->reportError(joint->m_name.c_str()); + delete joint; return false; } else @@ -1552,6 +1517,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF else { logger->reportError("joint xml is not initialized correctly"); + delete joint; return false; } } @@ -1666,7 +1632,8 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str()); if (mat) { - logger->reportWarning("Duplicate material"); + logger->reportWarning("Duplicate material"); + delete material; } else { localModel->m_materials.insert(material->m_name.c_str(),material); @@ -1689,6 +1656,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) { logger->reportError("Link name is not unique, link names in the same model have to be unique"); logger->reportError(link->m_name.c_str()); + delete link; return false; } else { @@ -1737,6 +1705,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) { logger->reportError("joint '%s' is not unique."); logger->reportError(joint->m_name.c_str()); + delete joint; return false; } else @@ -1747,6 +1716,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) else { logger->reportError("joint xml is not initialized correctly"); + delete joint; return false; } } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index c22745b39..0bb291e58 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -195,7 +195,37 @@ struct UrdfModel { m_rootTransformInWorld.setIdentity(); } - + + ~UrdfModel() + { + for (int i = 0; i < m_materials.size(); i++) + { + UrdfMaterial** ptr = m_materials.getAtIndex(i); + if (ptr) + { + UrdfMaterial* t = *ptr; + delete t; + } + } + for (int i = 0; i < m_links.size(); i++) + { + UrdfLink** ptr = m_links.getAtIndex(i); + if (ptr) + { + UrdfLink* t = *ptr; + delete t; + } + } + for (int i = 0; i < m_joints.size(); i++) + { + UrdfJoint** ptr = m_joints.getAtIndex(i); + if (ptr) + { + UrdfJoint* t = *ptr; + delete t; + } + } + } }; class UrdfParser @@ -210,7 +240,6 @@ protected: int m_activeSdfModel; - void cleanModel(UrdfModel* model); bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger); bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger); bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger); diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index f0eaa8798..5e9579c08 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -526,7 +526,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( } else { - // We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape. + // We have to see something, take collision shape. Useful for MuJoCo xml, where there are no explicit visual shapes. useVisual = false; cnt = linkPtr->m_collisionArray.size(); } diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index b7636180c..7a33d01d1 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -260,7 +260,7 @@ public: inline int getSolveSwingLimit() { - return m_solveTwistLimit; + return m_solveSwingLimit; } inline btScalar getTwistLimitSign()