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

Merge pull request #414 from erwincoumans/master

Browse Source 
add alternative URDF parser that doens't use ROS urdf
This commit is contained in:
erwincoumans
2015-07-02 11:30:06 -07:00
parent 3da6763371 45d4816c44
commit 4a9441c5fd
19 changed files with 2368 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -85,8 +85,13 @@ SET(App_ExampleBrowser_SRCS
../Importers/ImportURDFDemo/URDF2Bullet.cpp ../Importers/ImportURDFDemo/URDF2Bullet.cpp
../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp ../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../Importers/ImportURDFDemo/MyMultiBodyCreator.h ../Importers/ImportURDFDemo/MyMultiBodyCreator.h
../Importers/ImportURDFDemo/MyURDFImporter.cpp ../Importers/ImportURDFDemo/ROSURDFImporter.cpp
../Importers/ImportURDFDemo/MyURDFImporter.h ../Importers/ImportURDFDemo/ROSURDFImporter.h
../Importers/ImportURDFDemo/UrdfParser.cpp
../Importers/ImportURDFDemo/urdfStringSplit.cpp
../Importers/ImportURDFDemo/urdfStringSplit.h
../Importers/ImportURDFDemo/BulletUrdfImporter.cpp
../Importers/ImportURDFDemo/BulletUrdfImporter.h
../VoronoiFracture/VoronoiFractureDemo.cpp ../VoronoiFracture/VoronoiFractureDemo.cpp
../VoronoiFracture/VoronoiFractureDemo.h ../VoronoiFracture/VoronoiFractureDemo.h
../VoronoiFracture/btConvexConvexMprAlgorithm.cpp ../VoronoiFracture/btConvexConvexMprAlgorithm.cpp

1
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -607,6 +607,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
///most OpenCL drivers and OpenCL compilers have issues with our kernels. ///most OpenCL drivers and OpenCL compilers have issues with our kernels.
///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers ///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers
///you could give it a try ///you could give it a try
///Note that several old OpenCL physics examples still have to be ported over to this new Example Browser
if (args.CheckCmdLineFlag("enable_experimental_opencl")) if (args.CheckCmdLineFlag("enable_experimental_opencl"))
{ {
gAllExamples->initOpenCLExampleEntries(); gAllExamples->initOpenCLExampleEntries();

918
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp Normal file
View File

@@ -0,0 +1,918 @@
/* Copyright (C) 2015 Google
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BulletUrdfImporter.h"
#include "URDFImporterInterface.h"
#include "btBulletCollisionCommon.h"
#include "../ImportObjDemo/LoadMeshFromObj.h"
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "Bullet3Common/b3FileUtils.h"
#include <string>
#include <iostream>
#include <fstream>
#include "UrdfParser.h"
struct BulletURDFInternalData
{
UrdfParser m_urdfParser;
struct GUIHelperInterface* m_guiHelper;
char m_pathPrefix[1024];
};
void BulletURDFImporter::printTree()
{
// btAssert(0);
}
enum MyFileType
{
FILE_STL=1,
FILE_COLLADA=2,
FILE_OBJ=3,
};
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper)
{
m_data = new BulletURDFInternalData;
m_data->m_guiHelper = helper;
m_data->m_pathPrefix[0]=0;
}
struct BulletErrorLogger : public ErrorLogger
{
int m_numErrors;
int m_numWarnings;
BulletErrorLogger()
:m_numErrors(0),
m_numWarnings(0)
{
}
virtual void reportError(const char* error)
{
m_numErrors++;
b3Error(error);
}
virtual void reportWarning(const char* warning)
{
m_numWarnings++;
b3Warning(warning);
}
virtual void printMessage(const char* msg)
{
b3Printf(msg);
}
};
bool BulletURDFImporter::loadURDF(const char* fileName)
{
//int argc=0;
char relativeFileName[1024];
b3FileUtils fu;
bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
std::string xml_string;
m_data->m_pathPrefix[0] = 0;
if (!fileFound){
std::cerr << "URDF file not found" << std::endl;
return false;
} else
{
int maxPathLen = 1024;
fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
std::fstream xml_file(relativeFileName, std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
}
BulletErrorLogger loggie;
bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(),&loggie);
return result;
}
const char* BulletURDFImporter::getPathPrefix()
{
return m_data->m_pathPrefix;
}
BulletURDFImporter::~BulletURDFImporter()
{
delete m_data;
}
int BulletURDFImporter::getRootLinkIndex() const
{
if (m_data->m_urdfParser.getModel().m_rootLinks.size()==1)
{
return m_data->m_urdfParser.getModel().m_rootLinks[0]->m_linkIndex;
}
return -1;
};
void BulletURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{
childLinkIndices.resize(0);
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
if (linkPtr)
{
const UrdfLink* link = *linkPtr;
//int numChildren = m_data->m_urdfParser->getModel().m_links.getAtIndex(linkIndex)->
for (int i=0;i<link->m_childLinks.size();i++)
{
int childIndex =link->m_childLinks[i]->m_linkIndex;
childLinkIndices.push_back(childIndex);
}
}
}
std::string BulletURDFImporter::getLinkName(int linkIndex) const
{
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
return link->m_name;
}
return "";
}
std::string BulletURDFImporter::getJointName(int linkIndex) const
{
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
if (link->m_parentJoint)
{
return link->m_parentJoint->m_name;
}
}
return "";
}
void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
//todo(erwincoumans)
//the link->m_inertia is NOT necessarily aligned with the inertial frame
//so an additional transform might need to be computed
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
mass = link->m_inertia.m_mass;
inertialFrame = link->m_inertia.m_linkLocalFrame;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
link->m_inertia.m_izz);
}
else
{
mass = 1.f;
localInertiaDiagonal.setValue(1,1,1);
inertialFrame.setIdentity();
}
}
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
{
jointLowerLimit = 0.f;
jointUpperLimit = 0.f;
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
if (link->m_parentJoint)
{
UrdfJoint* pj = link->m_parentJoint;
parent2joint = pj->m_parentLinkToJointTransform;
jointType = pj->m_type;
jointAxisInJointSpace = pj->m_localJointAxis;
jointLowerLimit = pj->m_lowerLimit;
jointUpperLimit = pj->m_upperLimit;
return true;
} else
{
parent2joint.setIdentity();
return false;
}
}
return false;
}
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
{
GLInstanceGraphicsShape* glmesh = 0;
btConvexShape* convexColShape = 0;
switch (visual->m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
{
printf("processing a cylinder\n");
btAlignedObjectArray<btVector3> vertices;
//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_BOX:
{
printf("processing a box\n");
btVector3 extents = visual->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
convexColShape = boxShape;
convexColShape->setMargin(0.001);
break;
}
case URDF_GEOM_SPHERE:
{
printf("processing a sphere\n");
btScalar radius = visual->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
convexColShape = sphereShape;
convexColShape->setMargin(0.001);
break;
break;
}
case URDF_GEOM_MESH:
{
if (visual->m_name.length())
{
printf("visual->name=%s\n", visual->m_name.c_str());
}
if (1)//visual->m_geometry)
{
if (visual->m_geometry.m_meshFileName.length())
{
const char* filename = visual->m_geometry.m_meshFileName.c_str();
printf("mesh->filename=%s\n", filename);
char fullPath[1024];
int fileType = 0;
char tmpPathPrefix[1024];
std::string xml_string;
int maxPathLen = 1024;
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
char visualPathPrefix[1024];
sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
b3FileUtils::toLower(fullPath);
if (strstr(fullPath, ".dae"))
{
fileType = FILE_COLLADA;
}
if (strstr(fullPath, ".stl"))
{
fileType = FILE_STL;
}
if (strstr(fullPath,".obj"))
{
fileType = FILE_OBJ;
}
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
FILE* f = fopen(fullPath, "rb");
if (f)
{
fclose(f);
switch (fileType)
{
case FILE_OBJ:
{
glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
break;
}
case FILE_STL:
{
glmesh = LoadMeshFromSTL(fullPath);
break;
}
case FILE_COLLADA:
{
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
btTransform upAxisTrans; upAxisTrans.setIdentity();
float unitMeterScaling = 1;
int upAxis = 2;
LoadMeshFromCollada(fullPath,
visualShapes,
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis);
glmesh = new GLInstanceGraphicsShape;
int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i = 0; i<visualShapeInstances.size(); i++)
{
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
b3AlignedObjectArray<GLInstanceVertex> verts;
verts.resize(gfxShape->m_vertices->size());
int baseIndex = glmesh->m_vertices->size();
for (int i = 0; i<gfxShape->m_vertices->size(); i++)
{
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
}
int curNumIndices = glmesh->m_indices->size();
int additionalIndices = gfxShape->m_indices->size();
glmesh->m_indices->resize(curNumIndices + additionalIndices);
for (int k = 0; k<additionalIndices; k++)
{
glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
}
//compensate upAxisTrans and unitMeterScaling here
btMatrix4x4 upAxisMat;
upAxisMat.setIdentity();
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
btMatrix4x4 unitMeterScalingMat;
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
//btMatrix4x4 worldMat = instance->m_worldTransform;
int curNumVertices = glmesh->m_vertices->size();
int additionalVertices = verts.size();
glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
for (int v = 0; v<verts.size(); v++)
{
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
pos = worldMat*pos;
verts[v].xyzw[0] = float(pos[0]);
verts[v].xyzw[1] = float(pos[1]);
verts[v].xyzw[2] = float(pos[2]);
glmesh->m_vertices->push_back(verts[v]);
}
}
glmesh->m_numIndices = glmesh->m_indices->size();
glmesh->m_numvertices = glmesh->m_vertices->size();
//glmesh = LoadMeshFromCollada(fullPath);
break;
}
default:
{
printf("Error: unsupported file type for Visual mesh: %s\n", fullPath);
btAssert(0);
}
}
if (glmesh && (glmesh->m_numvertices>0))
{
}
else
{
printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
}
}
else
{
printf("mesh geometry not found %s\n", fullPath);
}
}
}
break;
}
default:
{
printf("Error: unknown visual geometry type\n");
}
}
//if we have a convex, tesselate into localVertices/localIndices
if (convexColShape)
{
btShapeHull* hull = new btShapeHull(convexColShape);
hull->buildHull(0.0);
{
// int strideInBytes = 9*sizeof(float);
int numVertices = hull->numVertices();
int numIndices = hull->numIndices();
glmesh = new GLInstanceGraphicsShape;
int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i = 0; i < numVertices; i++)
{
GLInstanceVertex vtx;
btVector3 pos = hull->getVertexPointer()[i];
vtx.xyzw[0] = pos.x();
vtx.xyzw[1] = pos.y();
vtx.xyzw[2] = pos.z();
vtx.xyzw[3] = 1.f;
pos.normalize();
vtx.normal[0] = pos.x();
vtx.normal[1] = pos.y();
vtx.normal[2] = pos.z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
glmesh->m_vertices->push_back(vtx);
}
btAlignedObjectArray<int> indices;
for (int i = 0; i < numIndices; i++)
{
glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
}
glmesh->m_numvertices = glmesh->m_vertices->size();
glmesh->m_numIndices = glmesh->m_indices->size();
}
delete convexColShape;
convexColShape = 0;
}
if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
{
int baseIndex = verticesOut.size();
for (int i = 0; i < glmesh->m_indices->size(); i++)
{
indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
}
for (int i = 0; i < glmesh->m_vertices->size(); i++)
{
GLInstanceVertex& v = glmesh->m_vertices->at(i);
btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
btVector3 vt = visualTransform*vert;
v.xyzw[0] = vt[0];
v.xyzw[1] = vt[1];
v.xyzw[2] = vt[2];
btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
triNormal = visualTransform.getBasis()*triNormal;
v.normal[0] = triNormal[0];
v.normal[1] = triNormal[1];
v.normal[2] = triNormal[2];
verticesOut.push_back(v);
}
}
}
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
{
btCollisionShape* shape = 0;
switch (collision->m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
{
printf("processing a cylinder\n");
btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
btScalar cylLength = collision->m_geometry.m_cylinderLength;
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i=0;i<numSteps;i++)
{
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);
cylZShape->initializePolyhedralFeatures();
//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
shape = cylZShape;
break;
}
case URDF_GEOM_BOX:
{
printf("processing a box\n");
btVector3 extents = collision->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
shape = boxShape;
shape ->setMargin(0.001);
break;
}
case URDF_GEOM_SPHERE:
{
printf("processing a sphere\n");
btScalar radius = collision->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
shape = sphereShape;
shape ->setMargin(0.001);
break;
break;
}
case URDF_GEOM_MESH:
{
if (collision->m_name.length())
{
printf("collision->name=%s\n",collision->m_name.c_str());
}
if (1)
{
if (collision->m_geometry.m_meshFileName.length())
{
const char* filename = collision->m_geometry.m_meshFileName.c_str();
printf("mesh->filename=%s\n",filename);
char fullPath[1024];
int fileType = 0;
sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
b3FileUtils::toLower(fullPath);
char tmpPathPrefix[1024];
int maxPathLen = 1024;
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
char collisionPathPrefix[1024];
sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
if (strstr(fullPath,".dae"))
{
fileType = FILE_COLLADA;
}
if (strstr(fullPath,".stl"))
{
fileType = FILE_STL;
}
if (strstr(fullPath,".obj"))
{
fileType = FILE_OBJ;
}
sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
FILE* f = fopen(fullPath,"rb");
if (f)
{
fclose(f);
GLInstanceGraphicsShape* glmesh = 0;
switch (fileType)
{
case FILE_OBJ:
{
glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix);
break;
}
case FILE_STL:
{
glmesh = LoadMeshFromSTL(fullPath);
break;
}
case FILE_COLLADA:
{
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
btTransform upAxisTrans;upAxisTrans.setIdentity();
float unitMeterScaling=1;
int upAxis = 2;
LoadMeshFromCollada(fullPath,
visualShapes,
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis );
glmesh = new GLInstanceGraphicsShape;
int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i=0;i<visualShapeInstances.size();i++)
{
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
b3AlignedObjectArray<GLInstanceVertex> verts;
verts.resize(gfxShape->m_vertices->size());
int baseIndex = glmesh->m_vertices->size();
for (int i=0;i<gfxShape->m_vertices->size();i++)
{
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
}
int curNumIndices = glmesh->m_indices->size();
int additionalIndices = gfxShape->m_indices->size();
glmesh->m_indices->resize(curNumIndices+additionalIndices);
for (int k=0;k<additionalIndices;k++)
{
glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
}
//compensate upAxisTrans and unitMeterScaling here
btMatrix4x4 upAxisMat;
upAxisMat.setPureRotation(upAxisTrans.getRotation());
btMatrix4x4 unitMeterScalingMat;
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
//btMatrix4x4 worldMat = instance->m_worldTransform;
int curNumVertices = glmesh->m_vertices->size();
int additionalVertices = verts.size();
glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
for(int v=0;v<verts.size();v++)
{
btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
pos = worldMat*pos;
verts[v].xyzw[0] = float(pos[0]);
verts[v].xyzw[1] = float(pos[1]);
verts[v].xyzw[2] = float(pos[2]);
glmesh->m_vertices->push_back(verts[v]);
}
}
glmesh->m_numIndices = glmesh->m_indices->size();
glmesh->m_numvertices = glmesh->m_vertices->size();
//glmesh = LoadMeshFromCollada(fullPath);
break;
}
default:
{
printf("Unsupported file type in Collision: %s\n",fullPath);
btAssert(0);
}
}
if (glmesh && (glmesh->m_numvertices>0))
{
printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
//convex->setUserIndex(shapeId);
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],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2]));
}
//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
//cylZShape->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
shape = cylZShape;
} else
{
printf("issue extracting mesh from STL file %s\n", fullPath);
}
} else
{
printf("mesh geometry not found %s\n",fullPath);
}
}
}
break;
}
default:
{
printf("Error: unknown visual geometry type\n");
}
}
return shape;
}
int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
{
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
int graphicsIndex = -1;
#if USE_ROS_URDF_PARSER
for (int v = 0; v < (int)m_data->m_links[linkIndex]->visual_array.size(); v++)
{
const Visual* vis = m_data->m_links[linkIndex]->visual_array[v].get();
btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
convertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
}
#else
const UrdfModel& model = m_data->m_urdfParser.getModel();
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
if (linkPtr)
{
const UrdfLink* link = *linkPtr;
for (int v = 0; v < link->m_visualArray.size();v++)
{
const UrdfVisual& vis = link->m_visualArray[v];
btTransform childTrans = vis.m_linkLocalFrame;
convertURDFToVisualShape(&vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
}
}
#endif
if (vertices.size() && indices.size())
{
graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
}
return graphicsIndex;
}
class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
btCompoundShape* compoundShape = new btCompoundShape();
compoundShape->setMargin(0.001);
#if USE_ROS_URDF_PARSER
for (int v=0;v<(int)m_data->m_links[linkIndex]->collision_array.size();v++)
{
const Collision* col = m_data->m_links[linkIndex]->collision_array[v].get();
btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
if (childShape)
{
btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
btTransform childTrans;
childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn);
compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
}
}
#else
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
for (int v=0;v<link->m_collisionArray.size();v++)
{
const UrdfCollision& col = link->m_collisionArray[v];
btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix);
if (childShape)
{
btTransform childTrans = col.m_linkLocalFrame;
compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
}
}
}
#endif
return compoundShape;
}

14
examples/Importers/ImportURDFDemo/MyURDFImporter.h → examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -1,20 +1,20 @@
#ifndef MY_URDF_IMPORTER_H #ifndef BULLET_URDF_IMPORTER_H
#define MY_URDF_IMPORTER_H #define BULLET_URDF_IMPORTER_H
#include "URDFImporterInterface.h" #include "URDFImporterInterface.h"
class MyURDFImporter : public URDFImporterInterface class BulletURDFImporter : public URDFImporterInterface
{ {
struct MyURDFInternalData* m_data; struct BulletURDFInternalData* m_data;
public: public:
MyURDFImporter(struct GUIHelperInterface* guiHelper); BulletURDFImporter(struct GUIHelperInterface* guiHelper);
virtual ~MyURDFImporter(); virtual ~BulletURDFImporter();
virtual bool loadURDF(const char* fileName); virtual bool loadURDF(const char* fileName);
@@ -41,4 +41,4 @@ public:
}; };
#endif //MY_URDF_IMPORTER_H #endif //BULLET_URDF_IMPORTER_H

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

@@ -9,7 +9,8 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "../CommonInterfaces/CommonParameterInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h"
#include "MyURDFImporter.h" #include "ROSURDFImporter.h"
#include "BulletUrdfImporter.h"
#include "URDF2Bullet.h" #include "URDF2Bullet.h"
@@ -124,7 +125,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
if (gFileNameArray.size()==0) if (gFileNameArray.size()==0)
{ {
gFileNameArray.push_back("r2d2.urdf"); gFileNameArray.push_back("r2d2.urdf");//husky/husky.urdf");
} }
@@ -194,12 +195,28 @@ void ImportUrdfSetup::initPhysics()
//now print the tree using the new interface //now print the tree using the new interface
MyURDFImporter u2b(m_guiHelper); URDFImporterInterface* bla=0;
static bool newURDF = false;
newURDF = !newURDF;
if (newURDF)
{
b3Printf("using new URDF\n");
bla = new BulletURDFImporter(m_guiHelper);
} else
{
b3Printf("using ROS URDF\n");
bla = new ROSURDFImporter(m_guiHelper);
}
URDFImporterInterface& u2b = *bla;
bool loadOk = u2b.loadURDF(m_fileName); bool loadOk = u2b.loadURDF(m_fileName);
if (loadOk) if (loadOk)
{ {
u2b.printTree(); printTree(u2b,u2b.getRootLinkIndex());
//u2b.printTree();
btTransform identityTrans; btTransform identityTrans;
identityTrans.setIdentity(); identityTrans.setIdentity();
@@ -223,8 +240,8 @@ void ImportUrdfSetup::initPhysics()
{ {
//create motors for each btMultiBody joint //create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<mb->getNumLinks();i++) for (int i=0;i<numLinks;i++)
{ {
int mbLinkIndex = i; int mbLinkIndex = i;
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
@@ -259,7 +276,8 @@ void ImportUrdfSetup::initPhysics()
if (1) if (1)
{ {
//create motors for each generic joint //create motors for each generic joint
for (int i=0;i<creation.getNum6DofConstraints();i++) int num6Dof = creation.getNum6DofConstraints();
for (int i=0;i<num6Dof;i++)
{ {
btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i); btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
if (c->getUserConstraintPtr()) if (c->getUserConstraintPtr())

46
examples/Importers/ImportURDFDemo/MyURDFImporter.cpp → examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp
View File

@@ -12,7 +12,7 @@ subject to the following restrictions:
*/ */
#include "MyURDFImporter.h" #include "ROSURDFImporter.h"
#include "URDFImporterInterface.h" #include "URDFImporterInterface.h"
@@ -30,13 +30,13 @@ subject to the following restrictions:
#include <fstream> #include <fstream>
using namespace urdf; using namespace urdf;
void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut); void ROSconvertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut);
btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix); btCollisionShape* ROSconvertURDFToCollisionShape(const Collision* visual, const char* pathPrefix);
static void printTreeInternal(my_shared_ptr<const Link> link,int level = 0) static void ROSprintTreeInternal(my_shared_ptr<const Link> link,int level = 0)
{ {
level+=2; level+=2;
int count = 0; int count = 0;
@@ -47,7 +47,7 @@ static void printTreeInternal(my_shared_ptr<const Link> link,int level = 0)
for(int j=0;j<level;j++) std::cout << " "; //indent for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl; std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
// first grandchild // first grandchild
printTreeInternal(*child,level); ROSprintTreeInternal(*child,level);
} }
else else
{ {
@@ -61,7 +61,7 @@ static void printTreeInternal(my_shared_ptr<const Link> link,int level = 0)
struct MyURDFInternalData struct ROSURDFInternalData
{ {
my_shared_ptr<ModelInterface> m_robot; my_shared_ptr<ModelInterface> m_robot;
std::vector<my_shared_ptr<Link> > m_links; std::vector<my_shared_ptr<Link> > m_links;
@@ -70,9 +70,9 @@ struct MyURDFInternalData
}; };
void MyURDFImporter::printTree() void ROSURDFImporter::printTree()
{ {
printTreeInternal(m_data->m_robot->getRoot(),0); ROSprintTreeInternal(m_data->m_robot->getRoot(),0);
} }
enum MyFileType enum MyFileType
@@ -84,9 +84,9 @@ enum MyFileType
MyURDFImporter::MyURDFImporter(struct GUIHelperInterface* helper) ROSURDFImporter::ROSURDFImporter(struct GUIHelperInterface* helper)
{ {
m_data = new MyURDFInternalData; m_data = new ROSURDFInternalData;
m_data->m_robot = 0; m_data->m_robot = 0;
m_data->m_guiHelper = helper; m_data->m_guiHelper = helper;
m_data->m_pathPrefix[0]=0; m_data->m_pathPrefix[0]=0;
@@ -95,7 +95,7 @@ MyURDFImporter::MyURDFImporter(struct GUIHelperInterface* helper)
} }
bool MyURDFImporter::loadURDF(const char* fileName) bool ROSURDFImporter::loadURDF(const char* fileName)
{ {
@@ -160,19 +160,19 @@ bool MyURDFImporter::loadURDF(const char* fileName)
return true; return true;
} }
const char* MyURDFImporter::getPathPrefix() const char* ROSURDFImporter::getPathPrefix()
{ {
return m_data->m_pathPrefix; return m_data->m_pathPrefix;
} }
MyURDFImporter::~MyURDFImporter() ROSURDFImporter::~ROSURDFImporter()
{ {
delete m_data; delete m_data;
} }
int MyURDFImporter::getRootLinkIndex() const int ROSURDFImporter::getRootLinkIndex() const
{ {
if (m_data->m_links.size()) if (m_data->m_links.size())
{ {
@@ -183,7 +183,7 @@ int MyURDFImporter::getRootLinkIndex() const
return -1; return -1;
}; };
void MyURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const void ROSURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{ {
childLinkIndices.resize(0); childLinkIndices.resize(0);
int numChildren = m_data->m_links[linkIndex]->child_links.size(); int numChildren = m_data->m_links[linkIndex]->child_links.size();
@@ -195,19 +195,19 @@ void MyURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int
} }
} }
std::string MyURDFImporter::getLinkName(int linkIndex) const std::string ROSURDFImporter::getLinkName(int linkIndex) const
{ {
std::string n = m_data->m_links[linkIndex]->name; std::string n = m_data->m_links[linkIndex]->name;
return n; return n;
} }
std::string MyURDFImporter::getJointName(int linkIndex) const std::string ROSURDFImporter::getJointName(int linkIndex) const
{ {
return m_data->m_links[linkIndex]->parent_joint->name; return m_data->m_links[linkIndex]->parent_joint->name;
} }
void MyURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const void ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{ {
if ((*m_data->m_links[linkIndex]).inertial) if ((*m_data->m_links[linkIndex]).inertial)
{ {
@@ -223,7 +223,7 @@ void MyURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3&
} }
} }
bool MyURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
{ {
jointLowerLimit = 0.f; jointLowerLimit = 0.f;
jointUpperLimit = 0.f; jointUpperLimit = 0.f;
@@ -279,7 +279,7 @@ bool MyURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint,
void convertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut) void ROSconvertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
{ {
@@ -843,7 +843,7 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
int MyURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const int ROSURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
{ {
btAlignedObjectArray<GLInstanceVertex> vertices; btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices; btAlignedObjectArray<int> indices;
@@ -859,7 +859,7 @@ int MyURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefi
childTrans.setOrigin(childPos); childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn); childTrans.setRotation(childOrn);
convertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices); ROSconvertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
} }
@@ -872,7 +872,7 @@ int MyURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefi
} }
class btCompoundShape* MyURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const class btCompoundShape* ROSURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{ {
btCompoundShape* compoundShape = new btCompoundShape(); btCompoundShape* compoundShape = new btCompoundShape();

44
examples/Importers/ImportURDFDemo/ROSURDFImporter.h Normal file
View File

@@ -0,0 +1,44 @@
#ifndef ROS_URDF_IMPORTER_H
#define ROS_URDF_IMPORTER_H
#include "URDFImporterInterface.h"
class ROSURDFImporter : public URDFImporterInterface
{
struct ROSURDFInternalData* m_data;
public:
ROSURDFImporter(struct GUIHelperInterface* guiHelper);
virtual ~ROSURDFImporter();
virtual bool loadURDF(const char* fileName);
virtual const char* getPathPrefix();
void printTree(); //for debugging
virtual int getRootLinkIndex() const;
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
virtual std::string getLinkName(int linkIndex) const;
virtual std::string getJointName(int linkIndex) const;
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
};
#endif //ROS_URDF_IMPORTER_H

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

@@ -35,6 +35,20 @@ static btVector3 selectColor2()
void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel) void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel)
{ {
btScalar mass;
btVector3 localInertia;
btTransform inertialFrame;
u2b.getMassAndInertia(linkIndex,mass,localInertia,inertialFrame);
std::string name = u2b.getLinkName(linkIndex);
for(int j=0;j<indentationLevel;j++)
printf(" "); //indent
printf("link %s mass=%f\n",name.c_str(),mass);
for(int j=0;j<indentationLevel;j++)
printf(" "); //indent
printf("local inertia:%f,%f,%f\n",localInertia[0],
localInertia[1],
localInertia[2]);
btAlignedObjectArray<int> childIndices; btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex,childIndices); u2b.getLinkChildIndices(linkIndex,childIndices);
@@ -46,6 +60,8 @@ void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationL
{ {
int childLinkIndex = childIndices[i]; int childLinkIndex = childIndices[i];
std::string name = u2b.getLinkName(childLinkIndex); std::string name = u2b.getLinkName(childLinkIndex);
for(int j=0;j<indentationLevel;j++) printf(" "); //indent for(int j=0;j<indentationLevel;j++) printf(" "); //indent
printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex); printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
// first grandchild // first grandchild

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

@@ -15,6 +15,10 @@ public:
virtual ~URDFImporterInterface() {} virtual ~URDFImporterInterface() {}
virtual bool loadURDF(const char* fileName)=0;
virtual const char* getPathPrefix()=0;
///return >=0 for the root link index, -1 if there is no root link ///return >=0 for the root link index, -1 if there is no root link
virtual int getRootLinkIndex() const = 0; virtual int getRootLinkIndex() const = 0;

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

@@ -2,7 +2,7 @@
#define URDF_JOINT_TYPES_H #define URDF_JOINT_TYPES_H
enum enum UrdfJointTypes
{ {
URDFRevoluteJoint=1, URDFRevoluteJoint=1,
URDFPrismaticJoint, URDFPrismaticJoint,

847
examples/Importers/ImportURDFDemo/UrdfParser.cpp Normal file
View File

@@ -0,0 +1,847 @@
#include "UrdfParser.h"
#include "tinyxml/tinyxml.h"
#include "urdfStringSplit.h"
#include "urdfLexicalCast.h"
UrdfParser::UrdfParser()
{
}
UrdfParser::~UrdfParser()
{
//todo(erwincoumans) delete memory
}
static bool parseVector4(btVector4& vec4, const std::string& vector_str)
{
vec4.setZero();
btArray<std::string> pieces;
btArray<float> rgba;
urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
for (unsigned int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (rgba.size() != 4)
{
return false;
}
vec4.setValue(rgba[0],rgba[1],rgba[2],rgba[3]);
return true;
}
static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger)
{
vec3.setZero();
btArray<std::string> pieces;
btArray<float> rgba;
urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
for (unsigned int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (rgba.size() != 3)
{
logger->reportWarning("Couldn't parse vector3");
return false;
}
vec3.setValue(rgba[0],rgba[1],rgba[2]);
return true;
}
bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, ErrorLogger* logger)
{
if (!config->Attribute("name"))
{
logger->reportError("Material must contain a name attribute");
return false;
}
material.m_name = config->Attribute("name");
// texture
TiXmlElement *t = config->FirstChildElement("texture");
if (t)
{
if (t->Attribute("filename"))
{
material.m_textureFilename = t->Attribute("filename");
}
}
if (material.m_textureFilename.length()==0)
{
//logger->reportWarning("material has no texture file name");
}
// color
TiXmlElement *c = config->FirstChildElement("color");
if (c)
{
if (c->Attribute("rgba"))
{
if (!parseVector4(material.m_rgbaColor,c->Attribute("rgba")))
{
std::string msg = material.m_name+" has no rgba";
logger->reportWarning(msg.c_str());
}
}
}
return true;
}
bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger)
{
tr.setIdentity();
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str)
{
parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
}
}
{
const char* rpy_str = xml->Attribute("rpy");
if (rpy_str != NULL)
{
btVector3 rpy;
if (parseVector3(rpy,std::string(rpy_str),logger))
{
double phi, the, psi;
double roll = rpy[0];
double pitch = rpy[1];
double yaw = rpy[2];
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
btQuaternion orn(
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
orn.normalize();
tr.setRotation(orn);
}
}
}
return true;
}
bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger)
{
inertia.m_linkLocalFrame.setIdentity();
inertia.m_mass = 0.f;
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(inertia.m_linkLocalFrame,o,logger))
{
return false;
}
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
logger->reportError("Inertial element must have a mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
logger->reportError("Inertial: mass element must have value attribute");
return false;
}
inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
logger->reportError("Inertial element must have inertia element");
return false;
}
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
return true;
}
bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger)
{
btAssert(g);
TiXmlElement *shape = g->FirstChildElement();
if (!shape)
{
logger->reportError("Geometry tag contains no child element.");
return false;
}
const std::string type_name = shape->ValueTStr().c_str();
if (type_name == "sphere")
{
geom.m_type = URDF_GEOM_SPHERE;
if (!shape->Attribute("radius"))
{
logger->reportError("Sphere shape must have a radius attribute");
return false;
} else
{
geom.m_sphereRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
}
}
else if (type_name == "box")
{
geom.m_type = URDF_GEOM_BOX;
if (!shape->Attribute("size"))
{
logger->reportError("box requires a size attribute");
} else
{
parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
}
}
else if (type_name == "cylinder")
{
geom.m_type = URDF_GEOM_CYLINDER;
if (!shape->Attribute("length") ||
!shape->Attribute("radius"))
{
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"));
}
else if (type_name == "mesh")
{
geom.m_type = URDF_GEOM_MESH;
if (!shape->Attribute("filename")) {
logger->reportError("Mesh must contain a filename attribute");
return false;
}
geom.m_meshFileName = shape->Attribute("filename");
if (shape->Attribute("scale"))
{
parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger);
} else
{
geom.m_meshScale.setValue(1,1,1);
}
}
else
{
logger->reportError("Unknown geometry type:");
logger->reportError(type_name.c_str());
return false;
}
return true;
}
bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config, ErrorLogger* logger)
{
collision.m_linkLocalFrame.setIdentity();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(collision.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(collision.m_geometry,geom,logger))
{
return false;
}
const char *name_char = config->Attribute("name");
if (name_char)
collision.m_name = name_char;
return true;
}
bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(visual.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(visual.m_geometry,geom,logger))
{
return false;
}
const char *name_char = config->Attribute("name");
if (name_char)
visual.m_name = name_char;
visual.m_hasLocalMaterial = false;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (mat)
{
// get material name
if (!mat->Attribute("name"))
{
logger->reportError("Visual material must contain a name attribute");
return false;
}
visual.m_materialName = mat->Attribute("name");
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
visual.m_hasLocalMaterial = true;
}
}
}
return true;
}
bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
{
logger->reportError("Link with no name");
return false;
}
link.m_name = linkName;
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
if (!parseInertia(link.m_inertia, i,logger))
{
logger->reportError("Could not parse inertial element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
} else
{
logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
link.m_inertia.m_mass = 1.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 1.f;
link.m_inertia.m_iyy = 1.f;
link.m_inertia.m_izz= 1.f;
logger->reportWarning(link.m_name.c_str());
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
UrdfVisual visual;
if (parseVisual(visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
else
{
logger->reportError("Could not parse visual element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
UrdfCollision col;
if (parseCollision(col, col_xml,logger))
{
link.m_collisionArray.push_back(col);
}
else
{
logger->reportError("Could not parse collision element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
return true;
}
bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
{
joint.m_lowerLimit = 0.f;
joint.m_upperLimit = 0.f;
joint.m_effortLimit = 0.f;
joint.m_velocityLimit = 0.f;
const char* lower_str = config->Attribute("lower");
if (lower_str)
{
joint.m_lowerLimit = urdfLexicalCast<double>(lower_str);
}
const char* upper_str = config->Attribute("upper");
if (upper_str)
{
joint.m_upperLimit = urdfLexicalCast<double>(upper_str);
}
// Get joint effort limit
const char* effort_str = config->Attribute("effort");
if (effort_str)
{
joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
}
// Get joint velocity limit
const char* velocity_str = config->Attribute("velocity");
if (velocity_str)
{
joint.m_velocityLimit = urdfLexicalCast<double>(velocity_str);
}
return true;
}
bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger)
{
// Get Joint Name
const char *name = config->Attribute("name");
if (!name)
{
logger->reportError("unnamed joint found");
return false;
}
joint.m_name = name;
joint.m_parentLinkToJointTransform.setIdentity();
// Get transform from Parent Link to Joint Frame
TiXmlElement *origin_xml = config->FirstChildElement("origin");
if (origin_xml)
{
if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml,logger))
{
logger->reportError("Malformed parent origin element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
// Get Parent Link
TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
const char *pname = parent_xml->Attribute("link");
if (!pname)
{
logger->reportError("no parent link name specified for Joint link. this might be the root?");
logger->reportError(joint.m_name.c_str());
return false;
}
else
{
joint.m_parentLinkName = std::string(pname);
}
}
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
const char *pname = child_xml->Attribute("link");
if (!pname)
{
logger->reportError("no child link name specified for Joint link [%s].");
logger->reportError(joint.m_name.c_str());
return false;
}
else
{
joint.m_childLinkName = std::string(pname);
}
}
// Get Joint type
const char* type_char = config->Attribute("type");
if (!type_char)
{
logger->reportError("joint [%s] has no type, check to see if it's a reference.");
logger->reportError(joint.m_name.c_str());
return false;
}
std::string type_str = type_char;
if (type_str == "planar")
joint.m_type = URDFPlanarJoint;
else if (type_str == "floating")
joint.m_type = URDFFloatingJoint;
else if (type_str == "revolute")
joint.m_type = URDFRevoluteJoint;
else if (type_str == "continuous")
joint.m_type = URDFContinuousJoint;
else if (type_str == "prismatic")
joint.m_type = URDFPrismaticJoint;
else if (type_str == "fixed")
joint.m_type = URDFFixedJoint;
else
{
logger->reportError("Joint ");
logger->reportError(joint.m_name.c_str());
logger->reportError("has unknown type:");
logger->reportError(type_str.c_str());
return false;
}
// Get Joint Axis
if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
logger->reportWarning(joint.m_name.c_str());
joint.m_localJointAxis.setValue(1,0,0);
}
else{
if (axis_xml->Attribute("xyz"))
{
if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger))
{
logger->reportError("Malformed axis element:");
logger->reportError(joint.m_name.c_str());
logger->reportError(" for joint:");
logger->reportError(axis_xml->Attribute("xyz"));
return false;
}
}
}
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
if (!parseJointLimits(joint, limit_xml,logger))
{
logger->reportError("Could not parse limit element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
else if (joint.m_type == URDFRevoluteJoint)
{
logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
logger->reportError(joint.m_name.c_str());
return false;
}
else if (joint.m_type == URDFPrismaticJoint)
{
logger->reportError("Joint is of type PRISMATIC without limits");
logger->reportError( joint.m_name.c_str());
return false;
}
joint.m_jointDamping = 0;
joint.m_jointFriction = 0;
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
// Get joint damping
const char* damping_str = prop_xml->Attribute("damping");
if (damping_str)
{
joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
}
// Get joint friction
const char* friction_str = prop_xml->Attribute("friction");
if (friction_str)
{
joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
}
if (damping_str == NULL && friction_str == NULL)
{
logger->reportError("joint dynamics element specified with no damping and no friction");
return false;
}
}
return true;
}
bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
{
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
btHashMap<btHashString,btHashString> parentLinkTree;
// loop through all joints, for every link, assign children links and children joints
for (int i=0;i<m_model.m_joints.size();i++)
{
UrdfJoint** jointPtr = m_model.m_joints.getAtIndex(i);
if (jointPtr)
{
UrdfJoint* joint = *jointPtr;
std::string parent_link_name = joint->m_parentLinkName;
std::string child_link_name = joint->m_childLinkName;
if (parent_link_name.empty() || child_link_name.empty())
{
logger->reportError("parent link or child link is empty for joint");
logger->reportError(joint->m_name.c_str());
return false;
}
UrdfLink** childLinkPtr = m_model.m_links.find(joint->m_childLinkName.c_str());
if (!childLinkPtr)
{
logger->reportError("Cannot find child link for joint ");
logger->reportError(joint->m_name.c_str());
return false;
}
UrdfLink* childLink = *childLinkPtr;
UrdfLink** parentLinkPtr = m_model.m_links.find(joint->m_parentLinkName.c_str());
if (!parentLinkPtr)
{
logger->reportError("Cannot find parent link for a joint");
logger->reportError(joint->m_name.c_str());
return false;
}
UrdfLink* parentLink = *parentLinkPtr;
childLink->m_parentLink = parentLink;
childLink->m_parentJoint = joint;
parentLink->m_childJoints.push_back(joint);
parentLink->m_childLinks.push_back(childLink);
parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str());
}
}
//search for children that have no parent, those are 'root'
for (int i=0;i<m_model.m_links.size();i++)
{
UrdfLink** linkPtr = m_model.m_links.getAtIndex(i);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
link->m_linkIndex = i;
if (!link->m_parentLink)
{
m_model.m_rootLinks.push_back(link);
}
}
}
if (m_model.m_rootLinks.size()>1)
{
logger->reportWarning("URDF file with multiple root links found");
}
if (m_model.m_rootLinks.size()==0)
{
logger->reportError("URDF without root link found");
return false;
}
return true;
}
bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger)
{
TiXmlDocument xml_doc;
xml_doc.Parse(urdfText);
if (xml_doc.Error())
{
logger->reportError(xml_doc.ErrorDesc());
xml_doc.ClearError();
return false;
}
TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
logger->reportError("expected a robot element");
return false;
}
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
logger->reportError("Expected a name for robot");
return false;
}
m_model.m_name = name;
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
UrdfMaterial* material = new UrdfMaterial;
parseMaterial(*material, material_xml, logger);
UrdfMaterial** mat =m_model.m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
} else
{
m_model.m_materials.insert(material->m_name.c_str(),material);
}
}
char msg[1024];
sprintf(msg,"Num materials=%d", m_model.m_materials.size());
logger->printMessage(msg);
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
UrdfLink* link = new UrdfLink;
if (parseLink(*link, link_xml,logger))
{
if (m_model.m_links.find(link->m_name.c_str()))
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
return false;
} else
{
//copy model material into link material, if link has no local material
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial)
{
UrdfMaterial** mat = m_model.m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
} else
{
logger->reportError("Cannot find material with name:");
logger->reportError(vis.m_materialName.c_str());
}
}
}
m_model.m_links.insert(link->m_name.c_str(),link);
}
} else
{
logger->reportError("failed to parse link");
delete link;
return false;
}
}
if (m_model.m_links.size() == 0)
{
logger->reportWarning("No links found in URDF file");
return false;
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
UrdfJoint* joint = new UrdfJoint;
if (parseJoint(*joint, joint_xml,logger))
{
if (m_model.m_joints.find(joint->m_name.c_str()))
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
return false;
}
else
{
m_model.m_joints.insert(joint->m_name.c_str(),joint);
}
}
else
{
logger->reportError("joint xml is not initialized correctly");
return false;
}
}
return initTreeAndRoot(logger);
}

167
examples/Importers/ImportURDFDemo/UrdfParser.h Normal file
View File

@@ -0,0 +1,167 @@
#ifndef URDF_PARSER_H
#define URDF_PARSER_H
#include "LinearMath/btTransform.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btHashMap.h"
#include "URDFJointTypes.h"
#define btArray btAlignedObjectArray
#include <string>
struct ErrorLogger
{
virtual void reportError(const char* error)=0;
virtual void reportWarning(const char* warning)=0;
virtual void printMessage(const char* msg)=0;
};
struct UrdfMaterial
{
std::string m_name;
std::string m_textureFilename;
btVector4 m_rgbaColor;
};
struct UrdfInertia
{
btTransform m_linkLocalFrame;
double m_mass;
double m_ixx,m_ixy,m_ixz,m_iyy,m_iyz,m_izz;
UrdfInertia()
{
m_linkLocalFrame.setIdentity();
m_mass = 0.f;
m_ixx=m_ixy=m_ixz=m_iyy=m_iyz=m_izz=0.f;
}
};
enum UrdfGeomTypes
{
URDF_GEOM_SPHERE=2,
URDF_GEOM_BOX,
URDF_GEOM_CYLINDER,
URDF_GEOM_MESH
};
struct UrdfGeometry
{
UrdfGeomTypes m_type;
double m_sphereRadius;
btVector3 m_boxSize;
double m_cylinderRadius;
double m_cylinderLength;
std::string m_meshFileName;
btVector3 m_meshScale;
};
struct UrdfVisual
{
btTransform m_linkLocalFrame;
UrdfGeometry m_geometry;
std::string m_name;
std::string m_materialName;
bool m_hasLocalMaterial;
UrdfMaterial m_localMaterial;
};
struct UrdfCollision
{
btTransform m_linkLocalFrame;
UrdfGeometry m_geometry;
std::string m_name;
};
struct UrdfJoint;
struct UrdfLink
{
std::string m_name;
UrdfInertia m_inertia;
btArray<UrdfVisual> m_visualArray;
btArray<UrdfCollision> m_collisionArray;
UrdfLink* m_parentLink;
UrdfJoint* m_parentJoint;
btArray<UrdfJoint*> m_childJoints;
btArray<UrdfLink*> m_childLinks;
int m_linkIndex;
UrdfLink()
:m_parentLink(0),
m_parentJoint(0)
{
}
};
struct UrdfJoint
{
std::string m_name;
UrdfJointTypes m_type;
btTransform m_parentLinkToJointTransform;
std::string m_parentLinkName;
std::string m_childLinkName;
btVector3 m_localJointAxis;
double m_lowerLimit;
double m_upperLimit;
double m_effortLimit;
double m_velocityLimit;
double m_jointDamping;
double m_jointFriction;
};
struct UrdfModel
{
std::string m_name;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> m_rootLinks;
};
class UrdfParser
{
protected:
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
bool parseVisual(UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
bool parseCollision(UrdfCollision& collision, class TiXmlElement* config, ErrorLogger* logger);
bool initTreeAndRoot(ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
bool parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
UrdfModel m_model;
public:
UrdfParser();
virtual ~UrdfParser();
bool loadUrdf(const char* urdfText, ErrorLogger* logger);
const UrdfModel& getModel() const
{
return m_model;
}
UrdfModel& getModel()
{
return m_model;
}
};
#endif

17
examples/Importers/ImportURDFDemo/urdfLexicalCast.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H
#define BOOST_REPLACEMENT_LEXICAL_CAST_H
#include <stdlib.h>
template <typename T> T urdfLexicalCast(const char* txt)
{
double result = atof(txt);
return result;
};
#endif

250
examples/Importers/ImportURDFDemo/urdfStringSplit.cpp Normal file
View File

@@ -0,0 +1,250 @@
#include <assert.h>
//#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "urdfStringSplit.h"
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, btAlignedObjectArray<std::string> separators)
{
assert(separators.size()==1);
if (separators.size()==1)
{
char** strArray = urdfStrSplit(vector_str.c_str(),separators[0].c_str());
int numSubStr = urdfStrArrayLen(strArray);
for (int i=0;i<numSubStr;i++)
pieces.push_back(std::string(strArray[i]));
urdfStrArrayFree(strArray);
}
}
btAlignedObjectArray<std::string> urdfIsAnyOf(const char* seps)
{
btAlignedObjectArray<std::string> strArray;
int numSeps = strlen(seps);
for (int i=0;i<numSeps;i++)
{
char sep2[2] = {0,0};
sep2[0] = seps[i];
strArray.push_back(sep2);
}
return strArray;
}
/* Append an item to a dynamically allocated array of strings. On failure,
return NULL, in which case the original array is intact. The item
string is dynamically copied. If the array is NULL, allocate a new
array. Otherwise, extend the array. Make sure the array is always
NULL-terminated. Input string might not be '\0'-terminated. */
char **urdfStrArrayAppend(char **array, size_t nitems, const char *item,
size_t itemlen)
{
/* Make a dynamic copy of the item. */
char *copy;
if (item == NULL)
copy = NULL;
else {
copy = (char*)malloc(itemlen + 1);
if (copy == NULL)
return NULL;
memcpy(copy, item, itemlen);
copy[itemlen] = '\0';
}
/* Extend array with one element. Except extend it by two elements,
in case it did not yet exist. This might mean it is a teeny bit
too big, but we don't care. */
array = (char**)realloc(array, (nitems + 2) * sizeof(array[0]));
if (array == NULL) {
free(copy);
return NULL;
}
/* Add copy of item to array, and return it. */
array[nitems] = copy;
array[nitems+1] = NULL;
return array;
}
/* Free a dynamic array of dynamic strings. */
void urdfStrArrayFree(char **array)
{
if (array == NULL)
return;
for (size_t i = 0; array[i] != NULL; ++i)
free(array[i]);
free(array);
}
/* Split a string into substrings. Return dynamic array of dynamically
allocated substrings, or NULL if there was an error. Caller is
expected to free the memory, for example with str_array_free. */
char **urdfStrSplit(const char *input, const char *sep)
{
size_t nitems = 0;
char **array = NULL;
const char *start = input;
const char *next = strstr(start, sep);
size_t seplen = strlen(sep);
const char *item;
size_t itemlen;
for (;;) {
next = strstr(start, sep);
if (next == NULL) {
/* Add the remaining string (or empty string, if input ends with
separator. */
char **newstr = urdfStrArrayAppend(array, nitems, start, strlen(start));
if (newstr == NULL) {
urdfStrArrayFree(array);
return NULL;
}
array = newstr;
++nitems;
break;
} else if (next == input) {
/* Input starts with separator. */
item = "";
itemlen = 0;
} else {
item = start;
itemlen = next - item;
}
char **newstr = urdfStrArrayAppend(array, nitems, item, itemlen);
if (newstr == NULL) {
urdfStrArrayFree(array);
return NULL;
}
array = newstr;
++nitems;
start = next + seplen;
}
if (nitems == 0) {
/* Input does not contain separator at all. */
assert(array == NULL);
array = urdfStrArrayAppend(array, nitems, input, strlen(input));
}
return array;
}
/* Return length of a NULL-delimited array of strings. */
size_t urdfStrArrayLen(char **array)
{
size_t len;
for (len = 0; array[len] != NULL; ++len)
continue;
return len;
}
#ifdef UNIT_TEST_STRING_SPLIT
#define MAX_OUTPUT 20
int main(void)
{
struct {
const char *input;
const char *sep;
char *output[MAX_OUTPUT];
} tab[] = {
/* Input is empty string. Output should be a list with an empty
string. */
{
"",
"and",
{
"",
NULL,
},
},
/* Input is exactly the separator. Output should be two empty
strings. */
{
"and",
"and",
{
"",
"",
NULL,
},
},
/* Input is non-empty, but does not have separator. Output should
be the same string. */
{
"foo",
"and",
{
"foo",
NULL,
},
},
/* Input is non-empty, and does have separator. */
{
"foo bar 1 and foo bar 2",
" and ",
{
"foo bar 1",
"foo bar 2",
NULL,
},
},
};
const int tab_len = sizeof(tab) / sizeof(tab[0]);
bool errors;
errors = false;
for (int i = 0; i < tab_len; ++i) {
printf("test %d\n", i);
char **output = str_split(tab[i].input, tab[i].sep);
if (output == NULL) {
fprintf(stderr, "output is NULL\n");
errors = true;
break;
}
size_t num_output = str_array_len(output);
printf("num_output %lu\n", (unsigned long) num_output);
size_t num_correct = str_array_len(tab[i].output);
if (num_output != num_correct) {
fprintf(stderr, "wrong number of outputs (%lu, not %lu)\n",
(unsigned long) num_output, (unsigned long) num_correct);
errors = true;
} else {
for (size_t j = 0; j < num_output; ++j) {
if (strcmp(tab[i].output[j], output[j]) != 0) {
fprintf(stderr, "output[%lu] is '%s' not '%s'\n",
(unsigned long) j, output[j], tab[i].output[j]);
errors = true;
break;
}
}
}
str_array_free(output);
printf("\n");
}
if (errors)
return EXIT_FAILURE;
return 0;
}
#endif//UNIT_TEST_STRING_SPLIT

30
examples/Importers/ImportURDFDemo/urdfStringSplit.h Normal file
View File

@@ -0,0 +1,30 @@
#ifndef STRING_SPLIT_H
#define STRING_SPLIT_H
#include <cstring>
#include <string>
#include "LinearMath/btAlignedObjectArray.h"
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, btAlignedObjectArray<std::string> separators);
btAlignedObjectArray<std::string> urdfIsAnyOf(const char* seps);
///The string split C code is by Lars Wirzenius
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
/* Split a string into substrings. Return dynamic array of dynamically
allocated substrings, or NULL if there was an error. Caller is
expected to free the memory, for example with str_array_free. */
char** urdfStrSplit(const char* input, const char* sep);
/* Free a dynamic array of dynamic strings. */
void urdfStrArrayFree(char** array);
/* Return length of a NULL-delimited array of strings. */
size_t urdfStrArrayLen(char** array);
#endif //STRING_SPLIT_H

2
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -364,7 +364,7 @@ void PhysicsClient::createClientCommand()
void PhysicsClient::stepSimulation(float deltaTime) void PhysicsClient::stepSimulation(float deltaTime)
{ {
btAssert(m_testBlock1); // btAssert(m_testBlock1);
if (m_testBlock1) if (m_testBlock1)
{ {

4
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -4,7 +4,7 @@
#include "PosixSharedMemory.h" #include "PosixSharedMemory.h"
#include "Win32SharedMemory.h" #include "Win32SharedMemory.h"
#include "../Importers/ImportURDFDemo/MyURDFImporter.h" #include "../Importers/ImportURDFDemo/ROSURDFImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h"
@@ -132,7 +132,7 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
bool useMultiBody, bool useFixedBase) bool useMultiBody, bool useFixedBase)
{ {
MyURDFImporter u2b(m_guiHelper); ROSURDFImporter u2b(m_guiHelper);
bool loadOk = u2b.loadURDF(fileName); bool loadOk = u2b.loadURDF(fileName);
if (loadOk) if (loadOk)
{ {

9
examples/SharedMemory/premake4.lua
View File

@@ -22,8 +22,13 @@ files {
"../Importers/ImportURDFDemo/MultiBodyCreationInterface.h", "../Importers/ImportURDFDemo/MultiBodyCreationInterface.h",
"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
"../Importers/ImportURDFDemo/MyMultiBodyCreator.h", "../Importers/ImportURDFDemo/MyMultiBodyCreator.h",
"../Importers/ImportURDFDemo/MyURDFImporter.cpp", "../Importers/ImportURDFDemo/ROSURDFImporter.cpp",
"../Importers/ImportURDFDemo/MyURDFImporter.h", "../Importers/ImportURDFDemo/ROSURDFImporter.h",
"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../Importers/ImportURDFDemo/BulletUrdfImporter.h",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/UrdfParser.h",
"../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/URDF2Bullet.h", "../Importers/ImportURDFDemo/URDF2Bullet.h",
"../Importers/ImportURDFDemo/URDFImporterInterface.h", "../Importers/ImportURDFDemo/URDFImporterInterface.h",

1
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp
View File

@@ -76,6 +76,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o
// texture // texture
TiXmlElement *t = config->FirstChildElement("texture"); TiXmlElement *t = config->FirstChildElement("texture");
if (t) if (t)
{ {
if (t->Attribute("filename")) if (t->Attribute("filename"))