From 1524004aa86c53edd94062e5d1f273a5201cd241 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Thu, 6 Apr 2017 04:25:56 +0300 Subject: [PATCH 1/5] MJCF: support for "axisangle", small code improvements --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 51 ++++++++++--------- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 3 ++ .../TinyRendererVisualShapeConverter.cpp | 2 +- 3 files changed, 30 insertions(+), 26 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 3c5329809..d8db32b0b 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -630,11 +630,7 @@ struct BulletMJCFImporterInternalData bool handledGeomType = false; UrdfGeometry geom; - - - const char* gType = link_xml->Attribute("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"); @@ -707,6 +703,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 +713,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 +803,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 +888,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 +962,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 +972,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) { @@ -1205,6 +1205,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..2df69c430 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); + assert(localInertiaDiagonal[0] < 1e10); + assert(localInertiaDiagonal[1] < 1e10); + assert(localInertiaDiagonal[2] < 1e10); } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); 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(); } From 5a40b5aaaedbae7065a68e491e33612abb6b81ed Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Sat, 6 May 2017 03:15:57 +0300 Subject: [PATCH 2/5] Compile fix --- examples/Importers/ImportURDFDemo/URDF2Bullet.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 2df69c430..fcee5a150 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -285,9 +285,9 @@ void ConvertURDF2BulletInternal( if (!(flags & CUF_USE_URDF_INERTIA)) { compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); - assert(localInertiaDiagonal[0] < 1e10); - assert(localInertiaDiagonal[1] < 1e10); - assert(localInertiaDiagonal[2] < 1e10); + btAssert(localInertiaDiagonal[0] < 1e10); + btAssert(localInertiaDiagonal[1] < 1e10); + btAssert(localInertiaDiagonal[2] < 1e10); } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); From b8afb4d7aa388e07d218685eceb9d949bd00fbb2 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Sat, 15 Apr 2017 21:42:25 +0300 Subject: [PATCH 3/5] Memory leaks --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 8 +++ .../Importers/ImportURDFDemo/UrdfParser.cpp | 54 +++++-------------- .../Importers/ImportURDFDemo/UrdfParser.h | 33 +++++++++++- 3 files changed, 51 insertions(+), 44 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index d8db32b0b..befb30032 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;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); From 2c6b2211fcce88aa3405f8cd68537a2656e81367 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Sat, 6 May 2017 15:58:10 +0300 Subject: [PATCH 4/5] Friction typo --- examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index befb30032..b6eb24b9a 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -660,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()) From acf57f25e64862486b5e7e61f6b66a27d9040670 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Sat, 6 May 2017 03:00:37 +0300 Subject: [PATCH 5/5] MJCF: change default link name to be more unique --- examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index b6eb24b9a..ce49cf6f5 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -1025,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;