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

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

@@ -75,16 +75,6 @@ void BulletURDFImporter::printTree()
// btAssert(0);
}
enum MyFileType
{
FILE_STL=1,
FILE_COLLADA=2,
FILE_OBJ=3,
};
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
{
m_data = new BulletURDFInternalData;
@@ -624,7 +614,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
GLInstanceGraphicsShape* glmesh = 0;
switch (collision->m_geometry.m_meshFileType)
{
case FILE_OBJ:
case UrdfGeometry::FILE_OBJ:
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0);
@@ -640,11 +630,11 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
}
break;
case FILE_STL:
case UrdfGeometry::FILE_STL:
glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str());
break;
case FILE_COLLADA:
case UrdfGeometry::FILE_COLLADA:
{
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
@@ -833,7 +823,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
{
switch (visual->m_geometry.m_meshFileType)
{
case FILE_OBJ:
case UrdfGeometry::FILE_OBJ:
{
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
@@ -852,13 +842,13 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
break;
}
case FILE_STL:
case UrdfGeometry::FILE_STL:
{
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
break;
}
case FILE_COLLADA:
case UrdfGeometry::FILE_COLLADA:
{
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
@@ -1128,15 +1118,17 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
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)
{
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

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 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

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

@@ -1,9 +1,14 @@
#ifndef LINK_VISUAL_SHAPES_CONVERTER_H
#define LINK_VISUAL_SHAPES_CONVERTER_H
struct UrdfLink;
struct UrdfModel;
class btTransform;
class btCollisionObject;
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

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
{
if (cache.m_bulletMultiBody==0)
@@ -469,8 +469,8 @@ void ConvertURDF2BulletInternal(
btVector4 color = selectColor2();//(0.0,0.0,0.5);
u2b.getLinkColor(urdfLinkIndex,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;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
@@ -487,7 +487,7 @@ void ConvertURDF2BulletInternal(
}
} 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
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 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,
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;
btTransform m_linkLocalFrame;
UrdfGeometry m_geometry;
std::string m_name;
};
struct UrdfVisual: UrdfShape
{
std::string m_materialName;
bool m_hasLocalMaterial;
UrdfMaterial m_localMaterial;
};
struct UrdfCollision
struct UrdfCollision: UrdfShape
{
std::string m_sourceFileLocation;
btTransform m_linkLocalFrame;
UrdfGeometry m_geometry;
std::string m_name;
int m_flags;
int m_collisionGroup;
int m_collisionMask;