From 41df15a46401a53772bcb651288312ae463c9020 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Sun, 19 Mar 2017 00:19:04 +0300 Subject: [PATCH] MJCF: fix B3_PI, fix colors, fix capsule without 'fromto' --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 27 ++++++++-- .../Importers/ImportURDFDemo/UrdfParser.cpp | 18 +++---- .../Importers/ImportURDFDemo/UrdfParser.h | 13 +++-- .../TinyRendererVisualShapeConverter.cpp | 50 ++++++++++++------- 4 files changed, 75 insertions(+), 33 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 0fb20ad28..b1cbdb5d3 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -155,6 +155,9 @@ struct BulletMJCFImporterInternalData // joint defaults std::string m_defaultJointLimited; + // geom defaults + std::string m_defaultGeomRgba; + //those collision shapes are deleted by caller (todo: make sure this happens!) btAlignedObjectArray m_allocatedCollisionShapes; @@ -270,6 +273,11 @@ struct BulletMJCFImporterInternalData { m_defaultCollisionMask = urdfLexicalCast(conAffinityStr); } + const char* rgba = child_xml->Attribute("rgba"); + if (rgba) + { + m_defaultGeomRgba = rgba; + } } } handled=true; @@ -404,8 +412,8 @@ struct BulletMJCFImporterInternalData if (sizes.size()==2) { // TODO angle units are in " - range[0] = sizes[0] * M_PI / 180; - range[1] = sizes[1] * M_PI / 180; + range[0] = sizes[0] * B3_PI / 180; + range[1] = sizes[1] * B3_PI / 180; } else { logger->reportWarning("Expected range[2] in joint with limits"); @@ -526,10 +534,23 @@ struct BulletMJCFImporterInternalData -// const char* rgba = link_xml->Attribute("rgba"); const char* gType = link_xml->Attribute("type"); const char* sz = link_xml->Attribute("size"); const char* posS = link_xml->Attribute("pos"); + + std::string rgba = m_defaultGeomRgba; + if (const char* rgbattr = link_xml->Attribute("rgba")) + { + rgba = rgbattr; + } + if (!rgba.empty()) + { + // "0 0.7 0.7 1" + parseVector4(geom.m_localMaterial.m_rgbaColor, rgba); + geom.m_hasLocalMaterial = true; + geom.m_localMaterial.m_name = rgba; + } + if (posS) { btVector3 pos(0,0,0); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index ad5247710..cc188e74d 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -575,7 +575,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* if (name_char) visual.m_name = name_char; - visual.m_hasLocalMaterial = false; + visual.m_geometry.m_hasLocalMaterial = false; // Material TiXmlElement *mat = config->FirstChildElement("material"); @@ -597,7 +597,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* matPtr->m_rgbaColor = rgba; visual.m_materialName = matPtr->m_name; - visual.m_hasLocalMaterial = true; + visual.m_geometry.m_hasLocalMaterial = true; } } else @@ -616,11 +616,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* TiXmlElement *c = mat->FirstChildElement("color"); if (t||c) { - if (parseMaterial(visual.m_localMaterial, mat,logger)) + if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger)) { - UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial); + UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial); model.m_materials.insert(matPtr->m_name.c_str(),matPtr); - visual.m_hasLocalMaterial = true; + visual.m_geometry.m_hasLocalMaterial = true; } } } @@ -1408,12 +1408,12 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF for (int i=0;im_visualArray.size();i++) { UrdfVisual& vis = link->m_visualArray.at(i); - if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str()) + if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str()) { UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str()); if (mat && *mat) { - vis.m_localMaterial = **mat; + vis.m_geometry.m_localMaterial = **mat; } else { //logger->reportError("Cannot find material with name:"); @@ -1603,12 +1603,12 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) for (int i=0;im_visualArray.size();i++) { UrdfVisual& vis = link->m_visualArray.at(i); - if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str()) + if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str()) { UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str()); if (mat && *mat) { - vis.m_localMaterial = **mat; + vis.m_geometry.m_localMaterial = **mat; } else { //logger->reportError("Cannot find material with name:"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 6c2e0e090..1967cd729 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -18,9 +18,13 @@ struct ErrorLogger struct UrdfMaterial { - std::string m_name; + std::string m_name; std::string m_textureFilename; - btVector4 m_rgbaColor; + btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a + UrdfMaterial(): + m_rgbaColor(0.8, 0.8, 0.8, 1) + { + } }; struct UrdfInertia @@ -79,6 +83,9 @@ struct UrdfGeometry int m_meshFileType; std::string m_meshFileName; btVector3 m_meshScale; + + UrdfMaterial m_localMaterial; + bool m_hasLocalMaterial; }; bool findExistingMeshFile(const std::string& urdf_path, std::string fn, @@ -96,8 +103,6 @@ struct UrdfShape struct UrdfVisual: UrdfShape { std::string m_materialName; - bool m_hasLocalMaterial; - UrdfMaterial m_localMaterial; }; struct UrdfCollision: UrdfShape diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 58fd7f13e..a3631e9e2 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -178,6 +178,12 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi visualShapeOut.m_dimensions[1] = 0; visualShapeOut.m_dimensions[2] = 0; visualShapeOut.m_meshAssetFileName[0] = 0; + if (visual->m_geometry.m_hasLocalMaterial) { + visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_rgbaColor[0]; + visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_rgbaColor[1]; + visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_rgbaColor[2]; + visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_rgbaColor[3]; + } GLInstanceGraphicsShape* glmesh = 0; @@ -215,23 +221,33 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi { btVector3 p1 = visual->m_geometry.m_capsuleFrom; btVector3 p2 = visual->m_geometry.m_capsuleTo; - btVector3 v = p2 - p1; - btVector3 center = (p2 + p1) * 0.5; - btScalar rad = visual->m_geometry.m_capsuleRadius; - btVector3 up_vector(0,0,1); - btVector3 dir = v.normalized(); - btVector3 axis = dir.cross(up_vector); - if (axis.fuzzyZero()) - { - axis = btVector3(0,0,1); + btTransform tr; + tr.setIdentity(); + btScalar rad, len; + if (visual->m_geometry.m_hasFromTo) { + btVector3 v = p2 - p1; + btVector3 center = (p2 + p1) * 0.5; + btVector3 up_vector(0,0,1); + btVector3 dir = v.normalized(); + btVector3 axis = dir.cross(up_vector); + if (axis.fuzzyZero()) + { + axis = btVector3(0,0,1); + } + else + { + axis.normalize(); + } + btQuaternion q(axis, -acos(dir.dot(up_vector))); + btTransform capsule_orient(q, center); + tr = visual->m_linkLocalFrame * capsule_orient; + len = v.length(); + rad = visual->m_geometry.m_capsuleRadius; + } else { + tr = visual->m_linkLocalFrame; + len = visual->m_geometry.m_capsuleHalfHeight; + rad = visual->m_geometry.m_capsuleRadius; } - else - { - axis.normalize(); - } - btQuaternion q(axis, -acos(dir.dot(up_vector))); - btTransform capsule_orient(q, center); - btTransform tr = visual->m_linkLocalFrame * capsule_orient; visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0]; visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1]; visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2]; @@ -239,7 +255,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1]; visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2]; visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3]; - visualShapeOut.m_dimensions[0] = v.length(); + visualShapeOut.m_dimensions[0] = len; visualShapeOut.m_dimensions[1] = rad; break; }