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: