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

URDF loader: fix MuJoCo xml load, also closes #993

Browse Source
This commit is contained in:
Oleg Klimov
2017-03-17 02:09:55 +03:00
parent 86bec45247
commit e8da7bb6f8
13 changed files with 259 additions and 250 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -30,12 +30,6 @@
#include <vector> #include <vector>
enum eMJCF_FILE_TYPE_ENUMS
{
MJCF_FILE_STL = 1,
MJCF_FILE_OBJ = 2
};
enum ePARENT_LINK_ENUMS enum ePARENT_LINK_ENUMS
{ {
BASE_LINK_INDEX=-1, BASE_LINK_INDEX=-1,
@@ -137,9 +131,11 @@ struct MyMJCFAsset
struct BulletMJCFImporterInternalData struct BulletMJCFImporterInternalData
{ {
GUIHelperInterface* m_guiHelper; GUIHelperInterface* m_guiHelper;
struct LinkVisualShapesConverter* m_customVisualShapesConverter;
char m_pathPrefix[1024]; char m_pathPrefix[1024];
std::string m_fileModelName; std::string m_sourceFileName; // with path
std::string m_fileModelName; // without path
btHashMap<btHashString,MyMJCFAsset> m_assets; btHashMap<btHashString,MyMJCFAsset> m_assets;
btAlignedObjectArray<UrdfModel*> m_models; btAlignedObjectArray<UrdfModel*> m_models;
@@ -150,6 +146,7 @@ struct BulletMJCFImporterInternalData
int m_activeModel; int m_activeModel;
int m_activeBodyUniqueId;
//todo: for full MJCF compatibility, we would need a stack of default values //todo: for full MJCF compatibility, we would need a stack of default values
int m_defaultCollisionGroup; int m_defaultCollisionGroup;
int m_defaultCollisionMask; int m_defaultCollisionMask;
@@ -160,12 +157,20 @@ struct BulletMJCFImporterInternalData
BulletMJCFImporterInternalData() BulletMJCFImporterInternalData()
:m_activeModel(-1), :m_activeModel(-1),
m_activeBodyUniqueId(-1),
m_defaultCollisionGroup(1), m_defaultCollisionGroup(1),
m_defaultCollisionMask(1), m_defaultCollisionMask(1),
m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
{ {
m_pathPrefix[0] = 0; m_pathPrefix[0] = 0;
} }
std::string sourceFileLocation(TiXmlElement* e)
{
char buf[1024];
snprintf(buf, sizeof(buf), "%s:%i", m_sourceFileName.c_str(), e->Row());
return buf;
}
const UrdfLink* getLink(int modelIndex, int linkIndex) const const UrdfLink* getLink(int modelIndex, int linkIndex) const
{ {
@@ -621,9 +626,14 @@ struct BulletMJCFImporterInternalData
MyMJCFAsset* assetPtr = m_assets[meshStr]; MyMJCFAsset* assetPtr = m_assets[meshStr];
if (assetPtr) if (assetPtr)
{ {
handledGeomType = true;
geom.m_type = URDF_GEOM_MESH; geom.m_type = URDF_GEOM_MESH;
geom.m_meshFileName = assetPtr->m_fileName; geom.m_meshFileName = assetPtr->m_fileName;
bool exists = findExistingMeshFile(
m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml),
&geom.m_meshFileName,
&geom.m_meshFileType);
handledGeomType = exists;
geom.m_meshScale.setValue(1,1,1); geom.m_meshScale.setValue(1,1,1);
//todo: parse mesh scale //todo: parse mesh scale
if (sz) if (sz)
@@ -1113,10 +1123,11 @@ struct BulletMJCFImporterInternalData
}; };
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper) BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
{ {
m_data = new BulletMJCFImporterInternalData(); m_data = new BulletMJCFImporterInternalData();
m_data->m_guiHelper = helper; m_data->m_guiHelper = helper;
m_data->m_customVisualShapesConverter = customConverter;
} }
BulletMJCFImporter::~BulletMJCFImporter() BulletMJCFImporter::~BulletMJCFImporter()
@@ -1135,7 +1146,8 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
b3FileUtils fu; b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024); //bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0); bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
m_data->m_sourceFileName = relativeFileName;
std::string xml_string; std::string xml_string;
m_data->m_pathPrefix[0] = 0; m_data->m_pathPrefix[0] = 0;
@@ -1399,21 +1411,26 @@ bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
return false; return false;
} }
void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
{ {
if (m_data->m_customVisualShapesConverter)
{
const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,inertialFrame, link, 0, colObj, objectIndex);
}
} }
void BulletMJCFImporter::setBodyUniqueId(int bodyId) void BulletMJCFImporter::setBodyUniqueId(int bodyId)
{ {
m_data->m_activeBodyUniqueId = bodyId;
} }
int BulletMJCFImporter::getBodyUniqueId() const int BulletMJCFImporter::getBodyUniqueId() const
{ {
return 0; b3Assert(m_data->m_activeBodyUniqueId != -1);
return m_data->m_activeBodyUniqueId;
} }
static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin) static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
{ {
btCompoundShape* compound = new btCompoundShape(); btCompoundShape* compound = new btCompoundShape();
@@ -1496,132 +1513,87 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
} }
case URDF_GEOM_MESH: case URDF_GEOM_MESH:
{ {
////////////////////// GLInstanceGraphicsShape* glmesh = 0;
if (1) switch (col->m_geometry.m_meshFileType)
{
if (col->m_geometry.m_meshFileName.length())
{ {
const char* filename = col->m_geometry.m_meshFileName.c_str(); case UrdfGeometry::FILE_OBJ:
//b3Printf("mesh->filename=%s\n",filename);
char fullPath[1024];
int fileType = 0;
sprintf(fullPath,"%s%s",pathPrefix,filename);
b3FileUtils::toLower(fullPath);
char tmpPathPrefix[1024];
int maxPathLen = 1024;
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
char collisionPathPrefix[1024];
sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix);
if (strstr(fullPath,".stl"))
{ {
fileType = MJCF_FILE_STL; if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str());
//create a convex hull for each shape, and store it in a btCompoundShape
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
}
break;
} }
if (strstr(fullPath,".obj")) case UrdfGeometry::FILE_STL:
{
fileType = MJCF_FILE_OBJ;
}
sprintf(fullPath,"%s%s",pathPrefix,filename);
FILE* f = fopen(fullPath,"rb");
if (f)
{ {
fclose(f); glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str());
GLInstanceGraphicsShape* glmesh = 0; break;
}
default:
switch (fileType) b3Warning("%s: Unsupported file type in Collision: %s (maybe .dae?)\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileType);
{ }
case MJCF_FILE_OBJ:
{
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
//create a convex hull for each shape, and store it in a btCompoundShape
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin); if (childShape)
{
} // okay!
break; }
} else if (!glmesh || glmesh->m_numvertices<=0)
case MJCF_FILE_STL: {
{ b3Warning("%s: cannot extract anything useful from mesh '%s'\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(fullPath); }
break; else
} {
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
default: //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
{ //convex->setUserIndex(shapeId);
b3Warning("Unsupported file type in Collision: %s\n",fullPath); btAlignedObjectArray<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices);
} for (int i=0;i<glmesh->m_numvertices;i++)
} {
convertedVerts.push_back(btVector3(
glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
}
if (!childShape && glmesh && (glmesh->m_numvertices>0)) if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
btTriangleMesh* meshInterface = new btTriangleMesh();
for (int i=0;i<glmesh->m_numIndices/3;i++)
{ {
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
//convex->setUserIndex(shapeId); float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
btAlignedObjectArray<btVector3> convertedVerts; meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
convertedVerts.reserve(glmesh->m_numvertices); btVector3(v1[0],v1[1],v1[2]),
for (int i=0;i<glmesh->m_numvertices;i++) btVector3(v2[0],v2[1],v2[2]));
{
convertedVerts.push_back(btVector3(
glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
}
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
btTriangleMesh* meshInterface = new btTriangleMesh();
for (int i=0;i<glmesh->m_numIndices/3;i++)
{
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
btVector3(v1[0],v1[1],v1[2]),
btVector3(v2[0],v2[1],v2[2]));
}
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
childShape = trimesh;
} else
{
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull();
//convexHull->initializePolyhedralFeatures();
convexHull->setMargin(m_data->m_defaultCollisionMargin);
childShape = convexHull;
}
} else
{
b3Warning("issue extracting mesh from STL file %s\n", fullPath);
} }
delete glmesh; btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
childShape = trimesh;
} else } else
{ {
b3Warning("mesh geometry not found %s\n",fullPath); btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull();
//convexHull->initializePolyhedralFeatures();
convexHull->setMargin(m_data->m_defaultCollisionMargin);
childShape = convexHull;
} }
} }
}
////////////////////// delete glmesh;
break; break;
} }
case URDF_GEOM_CAPSULE: case URDF_GEOM_CAPSULE:
{ {
//todo: convert fromto to btCapsuleShape + local btTransform //todo: convert fromto to btCapsuleShape + local btTransform
@@ -1636,7 +1608,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
btVector3 fromto[2] = {f,t}; btVector3 fromto[2] = {f,t};
btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius) btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius)
,btScalar(col->m_geometry.m_capsuleRadius)}; ,btScalar(col->m_geometry.m_capsuleRadius)};
btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2); btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2);
childShape = ms; childShape = ms;
} else } else
@@ -1647,11 +1619,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
} }
break; break;
} }
default: } // switch geom
{
}
}
if (childShape) if (childShape)
{ {
m_data->m_allocatedCollisionShapes.push_back(childShape); m_data->m_allocatedCollisionShapes.push_back(childShape);

5
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h
View File

@@ -18,9 +18,8 @@ class BulletMJCFImporter : public URDFImporterInterface
{ {
struct BulletMJCFImporterInternalData* m_data; struct BulletMJCFImporterInternalData* m_data;
public: public:
BulletMJCFImporter(struct GUIHelperInterface* helper); BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter);
virtual ~BulletMJCFImporter(); virtual ~BulletMJCFImporter();
virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger); virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
@@ -66,7 +65,7 @@ public:
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const; virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
virtual void setBodyUniqueId(int bodyId); virtual void setBodyUniqueId(int bodyId);
virtual int getBodyUniqueId() const; virtual int getBodyUniqueId() const;

51
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
View File

@@ -188,12 +188,12 @@ void ImportMJCFSetup::initPhysics()
m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2; m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100; //m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode( m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawContactPoints +btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb +btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits); );//+btIDebugDraw::DBG_DrawConstraintLimits);
if (m_guiHelper->getParameterInterface()) if (m_guiHelper->getParameterInterface())
@@ -203,20 +203,23 @@ void ImportMJCFSetup::initPhysics()
slider.m_maxVal = 10; slider.m_maxVal = 10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
} }
BulletMJCFImporter importer(m_guiHelper); BulletMJCFImporter importer(m_guiHelper, 0);
MyMJCFLogger logger; MyMJCFLogger logger;
bool result = importer.loadMJCF(m_fileName,&logger); bool result = importer.loadMJCF(m_fileName,&logger);
if (result) if (result)
{ {
btTransform rootTrans; btTransform rootTrans;
rootTrans.setIdentity(); rootTrans.setIdentity();
for (int m =0; m<importer.getNumModels();m++) for (int m =0; m<importer.getNumModels();m++)
{ {
importer.activateModel(m);
// normally used with PhysicsServerCommandProcessor that allocates unique ids to multibodies,
// emulate this behavior here:
importer.setBodyUniqueId(m);
importer.activateModel(m);
btMultiBody* mb = 0; btMultiBody* mb = 0;
@@ -226,18 +229,17 @@ void ImportMJCFSetup::initPhysics()
MyMultiBodyCreator creation(m_guiHelper); MyMultiBodyCreator creation(m_guiHelper);
rootTrans.setIdentity(); rootTrans.setIdentity();
importer.getRootTransformInWorld(rootTrans); importer.getRootTransformInWorld(rootTrans);
ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF); ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
mb = creation.getBulletMultiBody(); mb = creation.getBulletMultiBody();
if (mb) if (mb)
{ {
printf("first MJCF file converted!\n"); std::string* name =
std::string* name = new std::string(importer.getLinkName(
new std::string(importer.getLinkName( importer.getRootLinkIndex()));
importer.getRootLinkIndex())); m_nameMemory.push_back(name);
m_nameMemory.push_back(name);
#ifdef TEST_MULTIBODY_SERIALIZATION #ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(name->c_str(),name->c_str()); s->registerNameForPointer(name->c_str(),name->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION #endif//TEST_MULTIBODY_SERIALIZATION
@@ -287,10 +289,11 @@ void ImportMJCFSetup::initPhysics()
m_data->m_numMotors++; m_data->m_numMotors++;
} }
} }
} }
} else } else
{ {
// not multibody
if (1) if (1)
{ {
//create motors for each generic joint //create motors for each generic joint

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

@@ -75,16 +75,6 @@ void BulletURDFImporter::printTree()
// btAssert(0); // btAssert(0);
} }
enum MyFileType
{
FILE_STL=1,
FILE_COLLADA=2,
FILE_OBJ=3,
};
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter) BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
{ {
m_data = new BulletURDFInternalData; m_data = new BulletURDFInternalData;
@@ -624,7 +614,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
GLInstanceGraphicsShape* glmesh = 0; GLInstanceGraphicsShape* glmesh = 0;
switch (collision->m_geometry.m_meshFileType) switch (collision->m_geometry.m_meshFileType)
{ {
case FILE_OBJ: case UrdfGeometry::FILE_OBJ:
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{ {
glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0); glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0);
@@ -640,11 +630,11 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
} }
break; break;
case FILE_STL: case UrdfGeometry::FILE_STL:
glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str()); glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str());
break; break;
case FILE_COLLADA: case UrdfGeometry::FILE_COLLADA:
{ {
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes; btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances; btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
@@ -833,7 +823,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
{ {
switch (visual->m_geometry.m_meshFileType) switch (visual->m_geometry.m_meshFileType)
{ {
case FILE_OBJ: case UrdfGeometry::FILE_OBJ:
{ {
b3ImportMeshData meshData; b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
@@ -852,13 +842,13 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
break; break;
} }
case FILE_STL: case UrdfGeometry::FILE_STL:
{ {
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
break; break;
} }
case FILE_COLLADA: case UrdfGeometry::FILE_COLLADA:
{ {
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes; btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances; btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
@@ -1128,15 +1118,17 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
return false; return false;
} }
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
{ {
if (m_data->m_customVisualShapesConverter) if (m_data->m_customVisualShapesConverter)
{ {
const UrdfModel& model = m_data->m_urdfParser.getModel(); const UrdfModel& model = m_data->m_urdfParser.getModel();
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId); UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
if (linkPtr)
{
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId);
}
} }
} }
int BulletURDFImporter::getNumAllocatedCollisionShapes() const int BulletURDFImporter::getNumAllocatedCollisionShapes() const

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -51,7 +51,7 @@ public:
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const; virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation

7
examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h
View File

@@ -1,9 +1,14 @@
#ifndef LINK_VISUAL_SHAPES_CONVERTER_H #ifndef LINK_VISUAL_SHAPES_CONVERTER_H
#define LINK_VISUAL_SHAPES_CONVERTER_H #define LINK_VISUAL_SHAPES_CONVERTER_H
struct UrdfLink;
struct UrdfModel;
class btTransform;
class btCollisionObject;
struct LinkVisualShapesConverter struct LinkVisualShapesConverter
{ {
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0; virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex) =0;
}; };
#endif //LINK_VISUAL_SHAPES_CONVERTER_H #endif //LINK_VISUAL_SHAPES_CONVERTER_H

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

@@ -311,7 +311,7 @@ void ConvertURDF2BulletInternal(
//untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body); //untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
} else } else
{ {
if (cache.m_bulletMultiBody==0) if (cache.m_bulletMultiBody==0)
@@ -469,8 +469,8 @@ void ConvertURDF2BulletInternal(
btVector4 color = selectColor2();//(0.0,0.0,0.5); btVector4 color = selectColor2();//(0.0,0.0,0.5);
u2b.getLinkColor(urdfLinkIndex,color); u2b.getLinkColor(urdfLinkIndex,color);
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId()); u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame,col, u2b.getBodyUniqueId());
URDFLinkContactInfo contactInfo; URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
@@ -487,7 +487,7 @@ void ConvertURDF2BulletInternal(
} }
} else } else
{ {
//u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape); //u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape);
} }
} }

2
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -49,7 +49,7 @@ public:
///quick hack: need to rethink the API/dependencies of this ///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;} virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { } virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
virtual void setBodyUniqueId(int bodyId) {} virtual void setBodyUniqueId(int bodyId) {}
virtual int getBodyUniqueId() const { return 0;} virtual int getBodyUniqueId() const { return 0;}

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

@@ -85,26 +85,23 @@ bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
const std::string& error_message_prefix, const std::string& error_message_prefix,
std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
struct UrdfVisual struct UrdfShape
{ {
std::string m_sourceFileLocation; std::string m_sourceFileLocation;
btTransform m_linkLocalFrame; btTransform m_linkLocalFrame;
UrdfGeometry m_geometry; UrdfGeometry m_geometry;
std::string m_name; std::string m_name;
};
struct UrdfVisual: UrdfShape
{
std::string m_materialName; std::string m_materialName;
bool m_hasLocalMaterial; bool m_hasLocalMaterial;
UrdfMaterial m_localMaterial; UrdfMaterial m_localMaterial;
}; };
struct UrdfCollision: UrdfShape
struct UrdfCollision
{ {
std::string m_sourceFileLocation;
btTransform m_linkLocalFrame;
UrdfGeometry m_geometry;
std::string m_name;
int m_flags; int m_flags;
int m_collisionGroup; int m_collisionGroup;
int m_collisionMask; int m_collisionMask;

2
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -1067,4 +1067,4 @@ void PhysicsDirect::setTimeOut(double timeOutInSeconds)
double PhysicsDirect::getTimeOut() const double PhysicsDirect::getTimeOut() const
{ {
return m_data->m_timeOutInSeconds; return m_data->m_timeOutInSeconds;
} }

25
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1497,7 +1497,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
m_data->m_sdfRecentLoadedBodies.clear(); m_data->m_sdfRecentLoadedBodies.clear();
BulletMJCFImporter u2b(m_data->m_guiHelper); //, &m_data->m_visualConverter BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
bool useFixedBase = false; bool useFixedBase = false;
MyMJCFLogger2 logger; MyMJCFLogger2 logger;
@@ -4610,20 +4610,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
shapeIndex, shapeIndex,
visualShapeStoragePtr); visualShapeStoragePtr);
if (success) {
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
//m_visualConverter serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; }
serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED; hasStatus = true;
hasStatus = true; break;
break;
} }
case CMD_UPDATE_VISUAL_SHAPE: case CMD_UPDATE_VISUAL_SHAPE:
{ {

129
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -170,7 +170,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff
m_data->m_hasLightSpecularCoeff = true; m_data->m_hasLightSpecularCoeff = true;
} }
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut) void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
{ {
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type; visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
@@ -211,6 +211,38 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
convexColShape = cylZShape; convexColShape = cylZShape;
break; break;
} }
case URDF_GEOM_CAPSULE:
{
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);
}
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];
visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0];
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[1] = rad;
break;
}
case URDF_GEOM_BOX: case URDF_GEOM_BOX:
{ {
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_boxSize[0]; visualShapeOut.m_dimensions[0] = visual->m_geometry.m_boxSize[0];
@@ -228,7 +260,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
case URDF_GEOM_SPHERE: case URDF_GEOM_SPHERE:
{ {
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius; visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius;
btScalar radius = visual->m_geometry.m_sphereRadius; btScalar radius = visual->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius); btSphereShape* sphereShape = new btSphereShape(radius);
convexColShape = sphereShape; convexColShape = sphereShape;
@@ -244,14 +276,6 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1]; visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2]; visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0];
visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1];
visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2];
visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0];
visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1];
visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2];
visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3];
switch (visual->m_geometry.m_meshFileType) switch (visual->m_geometry.m_meshFileType)
{ {
case UrdfGeometry::FILE_OBJ: case UrdfGeometry::FILE_OBJ:
@@ -467,17 +491,30 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId) void TinyRendererVisualShapeConverter::convertVisualShapes(
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
const UrdfLink* linkPtr, const UrdfModel* model,
class btCollisionObject* colObj, int bodyUniqueId)
{ {
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
if (linkPtr) if (linkPtr)
{ {
const btArray<UrdfVisual>* shapeArray;
bool useVisual;
int cnt = 0;
if (linkPtr->m_visualArray.size() > 0)
{
useVisual = true;
cnt = linkPtr->m_visualArray.size();
}
else
{
// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
useVisual = false;
cnt = linkPtr->m_collisionArray.size();
}
const UrdfLink* link = *linkPtr; for (int v1=0; v1<cnt; v1++)
for (int v1 = 0; v1 < link->m_visualArray.size();v1++)
{ {
btAlignedObjectArray<MyTexture2> textures; btAlignedObjectArray<MyTexture2> textures;
btAlignedObjectArray<GLInstanceVertex> vertices; btAlignedObjectArray<GLInstanceVertex> vertices;
@@ -485,20 +522,28 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
btTransform startTrans; startTrans.setIdentity(); btTransform startTrans; startTrans.setIdentity();
//int graphicsIndex = -1; //int graphicsIndex = -1;
const UrdfVisual& vis = link->m_visualArray[v1]; const UrdfShape* vis;
btTransform childTrans = vis.m_linkLocalFrame; if (useVisual) {
btHashString matName(vis.m_materialName.c_str()); vis = &linkPtr->m_visualArray[v1];
UrdfMaterial *const * matPtr = model.m_materials[matName]; } else {
vis = &linkPtr->m_collisionArray[v1];
float rgbaColor[4] = {1,1,1,1}; }
btTransform childTrans = vis->m_linkLocalFrame;
if (matPtr)
float rgbaColor[4] = {1,1,1,1};
if (model && useVisual)
{ {
UrdfMaterial *const mat = *matPtr; btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());
for (int i=0;i<4;i++) UrdfMaterial*const* matPtr = model->m_materials[matName];
rgbaColor[i] = mat->m_rgbaColor[i]; if (matPtr)
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); {
//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); for (int i=0; i<4; i++)
{
rgbaColor[i] = (*matPtr)->m_rgbaColor[i];
}
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
}
} }
TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj]; TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
@@ -513,19 +558,19 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
b3VisualShapeData visualShape; b3VisualShapeData visualShape;
visualShape.m_objectUniqueId = bodyUniqueId; visualShape.m_objectUniqueId = bodyUniqueId;
visualShape.m_linkIndex = linkIndex; visualShape.m_linkIndex = linkIndex;
visualShape.m_localVisualFrame[0] = vis.m_linkLocalFrame.getOrigin()[0]; visualShape.m_localVisualFrame[0] = vis->m_linkLocalFrame.getOrigin()[0];
visualShape.m_localVisualFrame[1] = vis.m_linkLocalFrame.getOrigin()[1]; visualShape.m_localVisualFrame[1] = vis->m_linkLocalFrame.getOrigin()[1];
visualShape.m_localVisualFrame[2] = vis.m_linkLocalFrame.getOrigin()[2]; visualShape.m_localVisualFrame[2] = vis->m_linkLocalFrame.getOrigin()[2];
visualShape.m_localVisualFrame[3] = vis.m_linkLocalFrame.getRotation()[0]; visualShape.m_localVisualFrame[3] = vis->m_linkLocalFrame.getRotation()[0];
visualShape.m_localVisualFrame[4] = vis.m_linkLocalFrame.getRotation()[1]; visualShape.m_localVisualFrame[4] = vis->m_linkLocalFrame.getRotation()[1];
visualShape.m_localVisualFrame[5] = vis.m_linkLocalFrame.getRotation()[2]; visualShape.m_localVisualFrame[5] = vis->m_linkLocalFrame.getRotation()[2];
visualShape.m_localVisualFrame[6] = vis.m_linkLocalFrame.getRotation()[3]; visualShape.m_localVisualFrame[6] = vis->m_linkLocalFrame.getRotation()[3];
visualShape.m_rgbaColor[0] = rgbaColor[0]; visualShape.m_rgbaColor[0] = rgbaColor[0];
visualShape.m_rgbaColor[1] = rgbaColor[1]; visualShape.m_rgbaColor[1] = rgbaColor[1];
visualShape.m_rgbaColor[2] = rgbaColor[2]; visualShape.m_rgbaColor[2] = rgbaColor[2];
visualShape.m_rgbaColor[3] = rgbaColor[3]; visualShape.m_rgbaColor[3] = rgbaColor[3];
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape); convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
m_data->m_visualShapes.push_back(visualShape); m_data->m_visualShapes.push_back(visualShape);
if (vertices.size() && indices.size()) if (vertices.size() && indices.size())

2
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@@ -16,7 +16,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
virtual ~TinyRendererVisualShapeConverter(); virtual ~TinyRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex); virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex);
virtual int getNumVisualShapes(int bodyUniqueId); virtual int getNumVisualShapes(int bodyUniqueId);