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

Merge pull request #1025 from olegklimov/master

Browse Source 
URDF loader improvement 2
This commit is contained in:
erwincoumans
2017-03-21 17:12:22 -07:00
committed by GitHub
parent 2b698688f4 80a0b51a1d
commit a30b887595
62 changed files with 398 additions and 331 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
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;
@@ -572,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));
@@ -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;
@@ -795,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);
@@ -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

@@ -315,7 +315,7 @@ void ConvertURDF2BulletInternal(
//untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body);
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
} else
{
if (cache.m_bulletMultiBody==0)
@@ -473,8 +473,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);
@@ -491,7 +491,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;}

23
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")
@@ -575,7 +576,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
if (name_char)
visual.m_name = name_char;
visual.m_hasLocalMaterial = false;
visual.m_geometry.m_hasLocalMaterial = false;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
@@ -597,7 +598,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
matPtr->m_rgbaColor = rgba;
visual.m_materialName = matPtr->m_name;
visual.m_hasLocalMaterial = true;
visual.m_geometry.m_hasLocalMaterial = true;
}
}
else
@@ -616,11 +617,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial);
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
visual.m_geometry.m_hasLocalMaterial = true;
}
}
}
@@ -1415,12 +1416,12 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
vis.m_geometry.m_localMaterial = **mat;
} else
{
//logger->reportError("Cannot find material with name:");
@@ -1610,12 +1611,12 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
vis.m_geometry.m_localMaterial = **mat;
} else
{
//logger->reportError("Cannot find material with name:");

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

@@ -18,9 +18,13 @@ struct ErrorLogger
struct UrdfMaterial
{
std::string m_name;
std::string m_name;
std::string m_textureFilename;
btVector4 m_rgbaColor;
btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a
UrdfMaterial():
m_rgbaColor(0.8, 0.8, 0.8, 1)
{
}
};
struct UrdfInertia
@@ -66,9 +70,6 @@ struct UrdfGeometry
btVector3 m_capsuleFrom;
btVector3 m_capsuleTo;
double m_cylinderRadius;
double m_cylinderLength;
btVector3 m_planeNormal;
enum {
@@ -79,32 +80,30 @@ struct UrdfGeometry
int m_meshFileType;
std::string m_meshFileName;
btVector3 m_meshScale;
UrdfMaterial m_localMaterial;
bool m_hasLocalMaterial;
};
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;
std::string m_materialName;
bool m_hasLocalMaterial;
UrdfMaterial m_localMaterial;
};
struct UrdfCollision
struct UrdfVisual: UrdfShape
{
std::string m_materialName;
};
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;