From 80a0b51a1de630eed2da187a0c2ca76551b7b774 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Wed, 22 Mar 2017 00:36:28 +0300 Subject: [PATCH] MJCF: fix cylinders in MJCF, also (possibly) fixes capsules in tiny renderer --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 18 ++------ .../ImportURDFDemo/BulletUrdfImporter.cpp | 8 ++-- .../Importers/ImportURDFDemo/UrdfParser.cpp | 5 +- .../Importers/ImportURDFDemo/UrdfParser.h | 3 -- .../TinyRendererVisualShapeConverter.cpp | 46 +++++++++---------- 5 files changed, 32 insertions(+), 48 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index b1cbdb5d3..563c657a4 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -618,10 +618,10 @@ struct BulletMJCFImporterInternalData handledGeomType = true; } - //todo: capsule, cylinder, meshes or heightfields etc - if (geomType == "capsule") + if (geomType == "capsule" || geomType == "cylinder") { - geom.m_type = URDF_GEOM_CAPSULE; + // + geom.m_type = geomType=="cylinder" ? URDF_GEOM_CYLINDER : URDF_GEOM_CAPSULE; btArray pieces; btArray sizes; @@ -695,13 +695,6 @@ struct BulletMJCFImporterInternalData } } } - #if 0 - if (geomType == "cylinder") - { - geom.m_type = URDF_GEOM_CYLINDER; - handledGeomType = true; - } -#endif if (handledGeomType) { @@ -866,6 +859,7 @@ struct BulletMJCFImporterInternalData return orgChildLinkIndex; } + bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger) { int newParentLinkIndex = orgParentLinkIndex; @@ -1027,10 +1021,6 @@ struct BulletMJCFImporterInternalData } linkPtr->m_linkTransformInWorld = linkTransform; - if (bodyN == "cart1")//front_left_leg") - { - printf("found!\n"); - } if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint) { //linkPtr->m_linkTransformInWorld.setIdentity(); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 2a60cb197..5ba817b51 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -562,8 +562,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co case URDF_GEOM_CYLINDER: { - btScalar cylRadius = collision->m_geometry.m_cylinderRadius; - btScalar cylLength = collision->m_geometry.m_cylinderLength; + btScalar cylRadius = collision->m_geometry.m_capsuleRadius; + btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight; btAlignedObjectArray vertices; //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); @@ -785,8 +785,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha for (int i = 0; im_geometry.m_cylinderRadius; - btScalar cylLength = visual->m_geometry.m_cylinderLength; + btScalar cylRadius = visual->m_geometry.m_capsuleRadius; + btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight; btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.); vertices.push_back(vert); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index cc188e74d..cbbc481c2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -401,8 +401,9 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger->reportError("Cylinder shape must have both length and radius attributes"); return false; } - geom.m_cylinderRadius = urdfLexicalCast(shape->Attribute("radius")); - geom.m_cylinderLength = urdfLexicalCast(shape->Attribute("length")); + geom.m_hasFromTo = false; + geom.m_capsuleRadius = urdfLexicalCast(shape->Attribute("radius")); + geom.m_capsuleHalfHeight = urdfLexicalCast(shape->Attribute("length")); } else if (type_name == "capsule") diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 1967cd729..015342a0a 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -70,9 +70,6 @@ struct UrdfGeometry btVector3 m_capsuleFrom; btVector3 m_capsuleTo; - double m_cylinderRadius; - double m_cylinderLength; - btVector3 m_planeNormal; enum { diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index a3631e9e2..520ca6710 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -192,31 +192,6 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi switch (visual->m_geometry.m_type) { case URDF_GEOM_CYLINDER: - { - btAlignedObjectArray vertices; - - visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength; - visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius; - - //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); - int numSteps = 32; - for (int i = 0; im_geometry.m_cylinderRadius; - btScalar cylLength = visual->m_geometry.m_cylinderLength; - - btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.); - vertices.push_back(vert); - vert[2] = -cylLength / 2.; - vertices.push_back(vert); - } - - btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - cylZShape->setMargin(0.001); - convexColShape = cylZShape; - break; - } case URDF_GEOM_CAPSULE: { btVector3 p1 = visual->m_geometry.m_capsuleFrom; @@ -257,6 +232,27 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3]; visualShapeOut.m_dimensions[0] = len; visualShapeOut.m_dimensions[1] = rad; + + btAlignedObjectArray vertices; + int numSteps = 32; + for (int i = 0; im_geometry.m_type==URDF_GEOM_CAPSULE) { + // TODO: check if tiny renderer works with that, didn't check -- Oleg + btVector3 pole1(0, 0, + len / 2. + rad); + btVector3 pole2(0, 0, - len / 2. - rad); + vertices.push_back(pole1); + vertices.push_back(pole2); + } + + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->setMargin(0.001); + convexColShape = cylZShape; break; } case URDF_GEOM_BOX: