Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

MJCF: fix cylinders in MJCF, also (possibly) fixes capsules in tiny renderer

Browse Source
This commit is contained in:
Oleg Klimov
2017-03-22 00:36:28 +03:00
parent 41df15a464
commit 80a0b51a1d
5 changed files with 32 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -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 conaffinity="0" contype="0" fromto="0 0 0 0 0 0.02" name="root" rgba="0.9 0.4 0.6 1" size=".011" type="cylinder"/>
geom.m_type = geomType=="cylinder" ? URDF_GEOM_CYLINDER : URDF_GEOM_CAPSULE;
btArray<std::string> pieces;
btArray<float> 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();

8
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -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<btVector3> 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; i<numSteps; i++)
{
btScalar cylRadius = visual->m_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);

5
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -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<double>(shape->Attribute("radius"));
geom.m_cylinderLength = urdfLexicalCast<double>(shape->Attribute("length"));
geom.m_hasFromTo = false;
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
}
else if (type_name == "capsule")

3
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -70,9 +70,6 @@ struct UrdfGeometry
btVector3 m_capsuleFrom;
btVector3 m_capsuleTo;
double m_cylinderRadius;
double m_cylinderLength;
btVector3 m_planeNormal;
enum {

46
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -192,31 +192,6 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
switch (visual->m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
{
btAlignedObjectArray<btVector3> 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; i<numSteps; i++)
{
btScalar cylRadius = visual->m_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<btVector3> vertices;
int numSteps = 32;
for (int i = 0; i<numSteps; i++)
{
btVector3 vert(rad*btSin(SIMD_2_PI*(float(i) / numSteps)), rad*btCos(SIMD_2_PI*(float(i) / numSteps)), len / 2.);
vertices.push_back(vert);
vert[2] = -len / 2.;
vertices.push_back(vert);
}
if (visual->m_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: