diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 916ec1143..4602046c5 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -85,8 +85,13 @@ SET(App_ExampleBrowser_SRCS ../Importers/ImportURDFDemo/URDF2Bullet.cpp ../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp ../Importers/ImportURDFDemo/MyMultiBodyCreator.h - ../Importers/ImportURDFDemo/MyURDFImporter.cpp - ../Importers/ImportURDFDemo/MyURDFImporter.h + ../Importers/ImportURDFDemo/ROSURDFImporter.cpp + ../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.h ../VoronoiFracture/btConvexConvexMprAlgorithm.cpp diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 6b682e0fd..948bfac47 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -607,6 +607,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) ///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 ///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")) { gAllExamples->initOpenCLExampleEntries(); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp new file mode 100644 index 000000000..ea2240150 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -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 + + + +#include +#include +#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& 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;im_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& verticesOut, btAlignedObjectArray& indicesOut) +{ + + + GLInstanceGraphicsShape* glmesh = 0; + + btConvexShape* convexColShape = 0; + + switch (visual->m_geometry.m_type) + { + case URDF_GEOM_CYLINDER: + { + printf("processing a cylinder\n"); + btAlignedObjectArray vertices; + + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i = 0; im_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 visualShapes; + btAlignedObjectArray 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(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_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; km_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; vm_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(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + + 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 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 vertices; + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i=0;isetMargin(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 visualShapes; + btAlignedObjectArray 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(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i=0;im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i=0;im_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;km_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;vm_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 convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + for (int i=0;im_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 vertices; + btAlignedObjectArray 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;vm_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; +} diff --git a/examples/Importers/ImportURDFDemo/MyURDFImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h similarity index 78% rename from examples/Importers/ImportURDFDemo/MyURDFImporter.h rename to examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index d36d42d11..d2493db89 100644 --- a/examples/Importers/ImportURDFDemo/MyURDFImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -1,20 +1,20 @@ -#ifndef MY_URDF_IMPORTER_H -#define MY_URDF_IMPORTER_H +#ifndef BULLET_URDF_IMPORTER_H +#define BULLET_URDF_IMPORTER_H #include "URDFImporterInterface.h" -class MyURDFImporter : public URDFImporterInterface +class BulletURDFImporter : public URDFImporterInterface { - struct MyURDFInternalData* m_data; + struct BulletURDFInternalData* m_data; public: - MyURDFImporter(struct GUIHelperInterface* guiHelper); + BulletURDFImporter(struct GUIHelperInterface* guiHelper); - virtual ~MyURDFImporter(); + virtual ~BulletURDFImporter(); virtual bool loadURDF(const char* fileName); @@ -41,4 +41,4 @@ public: }; -#endif //MY_URDF_IMPORTER_H +#endif //BULLET_URDF_IMPORTER_H diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 636460bae..a4f633e68 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -9,7 +9,8 @@ #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "../CommonInterfaces/CommonParameterInterface.h" -#include "MyURDFImporter.h" +#include "ROSURDFImporter.h" +#include "BulletUrdfImporter.h" #include "URDF2Bullet.h" @@ -124,7 +125,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, 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 - 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); if (loadOk) { - u2b.printTree(); + printTree(u2b,u2b.getRootLinkIndex()); + + //u2b.printTree(); btTransform identityTrans; identityTrans.setIdentity(); @@ -223,8 +240,8 @@ void ImportUrdfSetup::initPhysics() { //create motors for each btMultiBody joint - - for (int i=0;igetNumLinks();i++) + int numLinks = mb->getNumLinks(); + for (int i=0;igetLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute @@ -259,7 +276,8 @@ void ImportUrdfSetup::initPhysics() if (1) { //create motors for each generic joint - for (int i=0;igetUserConstraintPtr()) diff --git a/examples/Importers/ImportURDFDemo/MyURDFImporter.cpp b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp similarity index 93% rename from examples/Importers/ImportURDFDemo/MyURDFImporter.cpp rename to examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp index 71fd394d3..5caa0e2f5 100644 --- a/examples/Importers/ImportURDFDemo/MyURDFImporter.cpp +++ b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp @@ -12,7 +12,7 @@ subject to the following restrictions: */ -#include "MyURDFImporter.h" +#include "ROSURDFImporter.h" #include "URDFImporterInterface.h" @@ -30,13 +30,13 @@ subject to the following restrictions: #include using namespace urdf; -void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut); -btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix); +void ROSconvertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut); +btCollisionShape* ROSconvertURDFToCollisionShape(const Collision* visual, const char* pathPrefix); -static void printTreeInternal(my_shared_ptr link,int level = 0) +static void ROSprintTreeInternal(my_shared_ptr link,int level = 0) { level+=2; int count = 0; @@ -47,7 +47,7 @@ static void printTreeInternal(my_shared_ptr link,int level = 0) for(int j=0;jname << std::endl; // first grandchild - printTreeInternal(*child,level); + ROSprintTreeInternal(*child,level); } else { @@ -61,7 +61,7 @@ static void printTreeInternal(my_shared_ptr link,int level = 0) -struct MyURDFInternalData +struct ROSURDFInternalData { my_shared_ptr m_robot; std::vector > 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 @@ -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_guiHelper = helper; 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; } -const char* MyURDFImporter::getPathPrefix() +const char* ROSURDFImporter::getPathPrefix() { return m_data->m_pathPrefix; } -MyURDFImporter::~MyURDFImporter() +ROSURDFImporter::~ROSURDFImporter() { delete m_data; } -int MyURDFImporter::getRootLinkIndex() const +int ROSURDFImporter::getRootLinkIndex() const { if (m_data->m_links.size()) { @@ -183,7 +183,7 @@ int MyURDFImporter::getRootLinkIndex() const return -1; }; -void MyURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const +void ROSURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const { childLinkIndices.resize(0); int numChildren = m_data->m_links[linkIndex]->child_links.size(); @@ -195,19 +195,19 @@ void MyURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArraym_links[linkIndex]->name; 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; } -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) { @@ -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; 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& verticesOut, btAlignedObjectArray& indicesOut) +void ROSconvertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& 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 vertices; btAlignedObjectArray indices; @@ -859,7 +859,7 @@ int MyURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefi childTrans.setOrigin(childPos); 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(); diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h new file mode 100644 index 000000000..75c6c0256 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h @@ -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& 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 diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 7b05fc7b3..0fcc3d1a9 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -35,6 +35,20 @@ static btVector3 selectColor2() 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 childIndices; u2b.getLinkChildIndices(linkIndex,childIndices); @@ -46,6 +60,8 @@ void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationL { int childLinkIndex = childIndices[i]; std::string name = u2b.getLinkName(childLinkIndex); + + for(int j=0;j=0 for the root link index, -1 if there is no root link virtual int getRootLinkIndex() const = 0; diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 3024536e4..0c50d2655 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -2,7 +2,7 @@ #define URDF_JOINT_TYPES_H -enum +enum UrdfJointTypes { URDFRevoluteJoint=1, URDFPrismaticJoint, diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp new file mode 100644 index 000000000..f3feb0722 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -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 pieces; + btArray rgba; + urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i) + { + if (!pieces[i].empty()) + { + rgba.push_back(urdfLexicalCast(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 pieces; + btArray rgba; + urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i) + { + if (!pieces[i].empty()) + { + rgba.push_back(urdfLexicalCast(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(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(inertia_xml->Attribute("ixx")); + inertia.m_ixy = urdfLexicalCast(inertia_xml->Attribute("ixy")); + inertia.m_ixz = urdfLexicalCast(inertia_xml->Attribute("ixz")); + inertia.m_iyy = urdfLexicalCast(inertia_xml->Attribute("iyy")); + inertia.m_iyz = urdfLexicalCast(inertia_xml->Attribute("iyz")); + inertia.m_izz = urdfLexicalCast(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(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(shape->Attribute("radius")); + geom.m_cylinderLength = urdfLexicalCast(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(lower_str); + } + + const char* upper_str = config->Attribute("upper"); + if (upper_str) + { + joint.m_upperLimit = urdfLexicalCast(upper_str); + } + + + // Get joint effort limit + const char* effort_str = config->Attribute("effort"); + if (effort_str) + { + joint.m_effortLimit = urdfLexicalCast(effort_str); + } + + // Get joint velocity limit + const char* velocity_str = config->Attribute("velocity"); + if (velocity_str) + { + joint.m_velocityLimit = urdfLexicalCast(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(damping_str); + } + + // Get joint friction + const char* friction_str = prop_xml->Attribute("friction"); + if (friction_str) + { + joint.m_jointFriction = urdfLexicalCast(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 parentLinkTree; + + // loop through all joints, for every link, assign children links and children joints + for (int i=0;im_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;im_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;im_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); + +} \ No newline at end of file diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h new file mode 100644 index 000000000..55bf0aa99 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -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 + +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 m_visualArray; + btArray m_collisionArray; + UrdfLink* m_parentLink; + UrdfJoint* m_parentJoint; + + btArray m_childJoints; + btArray 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 m_materials; + btHashMap m_links; + btHashMap m_joints; + + btArray 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 + diff --git a/examples/Importers/ImportURDFDemo/urdfLexicalCast.h b/examples/Importers/ImportURDFDemo/urdfLexicalCast.h new file mode 100644 index 000000000..45a4fdf10 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/urdfLexicalCast.h @@ -0,0 +1,17 @@ +#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H +#define BOOST_REPLACEMENT_LEXICAL_CAST_H + +#include + + +template T urdfLexicalCast(const char* txt) +{ + double result = atof(txt); + return result; +}; + + + + +#endif + diff --git a/examples/Importers/ImportURDFDemo/urdfStringSplit.cpp b/examples/Importers/ImportURDFDemo/urdfStringSplit.cpp new file mode 100644 index 000000000..a5a1adf83 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/urdfStringSplit.cpp @@ -0,0 +1,250 @@ + + +#include +//#include +#include +#include +#include + +#include "urdfStringSplit.h" + +void urdfStringSplit( btAlignedObjectArray&pieces, const std::string& vector_str, btAlignedObjectArray 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 urdfIsAnyOf(const char* seps) + { + btAlignedObjectArray strArray; + + int numSeps = strlen(seps); + for (int i=0;i +#include +#include "LinearMath/btAlignedObjectArray.h" + + +void urdfStringSplit( btAlignedObjectArray&pieces, const std::string& vector_str, btAlignedObjectArray separators); + +btAlignedObjectArray 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 + diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index f4869046f..8aeae62f7 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -364,7 +364,7 @@ void PhysicsClient::createClientCommand() void PhysicsClient::stepSimulation(float deltaTime) { - btAssert(m_testBlock1); + // btAssert(m_testBlock1); if (m_testBlock1) { diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 6a7b65b55..6b05826da 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -4,7 +4,7 @@ #include "PosixSharedMemory.h" #include "Win32SharedMemory.h" -#include "../Importers/ImportURDFDemo/MyURDFImporter.h" +#include "../Importers/ImportURDFDemo/ROSURDFImporter.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.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) { - MyURDFImporter u2b(m_guiHelper); + ROSURDFImporter u2b(m_guiHelper); bool loadOk = u2b.loadURDF(fileName); if (loadOk) { diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 72d8040ce..91ae0bd9f 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -22,8 +22,13 @@ files { "../Importers/ImportURDFDemo/MultiBodyCreationInterface.h", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.h", - "../Importers/ImportURDFDemo/MyURDFImporter.cpp", - "../Importers/ImportURDFDemo/MyURDFImporter.h", + "../Importers/ImportURDFDemo/ROSURDFImporter.cpp", + "../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.h", "../Importers/ImportURDFDemo/URDFImporterInterface.h", diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp index a224146b3..c25aaf696 100644 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp +++ b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp @@ -76,6 +76,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o // texture TiXmlElement *t = config->FirstChildElement("texture"); + if (t) { if (t->Attribute("filename"))