add alternative URDF parser that doens't use ROS urdf
This commit is contained in:
Erwin Coumans
918
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
Normal file
918
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
Normal 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;
|
||||
std::string n = 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;
|
||||
}
|
||||
44
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
Normal file
44
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
Normal file
@@ -0,0 +1,44 @@
|
||||
#ifndef BULLET_URDF_IMPORTER_H
|
||||
#define BULLET_URDF_IMPORTER_H
|
||||
|
||||
#include "URDFImporterInterface.h"
|
||||
|
||||
|
||||
class BulletURDFImporter : public URDFImporterInterface
|
||||
{
|
||||
|
||||
struct BulletURDFInternalData* m_data;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
BulletURDFImporter(struct GUIHelperInterface* guiHelper);
|
||||
|
||||
virtual ~BulletURDFImporter();
|
||||
|
||||
virtual bool loadURDF(const char* fileName);
|
||||
|
||||
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 //BULLET_URDF_IMPORTER_H
|
||||
@@ -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,7 +195,10 @@ void ImportUrdfSetup::initPhysics()
|
||||
|
||||
|
||||
//now print the tree using the new interface
|
||||
MyURDFImporter u2b(m_guiHelper);
|
||||
|
||||
//BulletURDFImporter u2b(m_guiHelper);
|
||||
ROSURDFImporter u2b(m_guiHelper);
|
||||
|
||||
bool loadOk = u2b.loadURDF(m_fileName);
|
||||
|
||||
if (loadOk)
|
||||
|
||||
@@ -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 <fstream>
|
||||
using namespace urdf;
|
||||
|
||||
void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut);
|
||||
btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix);
|
||||
void ROSconvertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut);
|
||||
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;
|
||||
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
|
||||
std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
|
||||
// first grandchild
|
||||
printTreeInternal(*child,level);
|
||||
ROSprintTreeInternal(*child,level);
|
||||
}
|
||||
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;
|
||||
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
|
||||
@@ -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<int>& childLinkIndices) const
|
||||
void ROSURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
|
||||
{
|
||||
childLinkIndices.resize(0);
|
||||
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;
|
||||
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<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<int> 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();
|
||||
@@ -1,20 +1,20 @@
|
||||
#ifndef MY_URDF_IMPORTER_H
|
||||
#define MY_URDF_IMPORTER_H
|
||||
#ifndef ROS_URDF_IMPORTER_H
|
||||
#define ROS_URDF_IMPORTER_H
|
||||
|
||||
#include "URDFImporterInterface.h"
|
||||
|
||||
|
||||
class MyURDFImporter : public URDFImporterInterface
|
||||
class ROSURDFImporter : public URDFImporterInterface
|
||||
{
|
||||
|
||||
struct MyURDFInternalData* m_data;
|
||||
struct ROSURDFInternalData* m_data;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
MyURDFImporter(struct GUIHelperInterface* guiHelper);
|
||||
ROSURDFImporter(struct GUIHelperInterface* guiHelper);
|
||||
|
||||
virtual ~MyURDFImporter();
|
||||
virtual ~ROSURDFImporter();
|
||||
|
||||
virtual bool loadURDF(const char* fileName);
|
||||
|
||||
@@ -41,4 +41,4 @@ public:
|
||||
};
|
||||
|
||||
|
||||
#endif //MY_URDF_IMPORTER_H
|
||||
#endif //ROS_URDF_IMPORTER_H
|
||||
@@ -2,7 +2,7 @@
|
||||
#define URDF_JOINT_TYPES_H
|
||||
|
||||
|
||||
enum
|
||||
enum UrdfJointTypes
|
||||
{
|
||||
URDFRevoluteJoint=1,
|
||||
URDFPrismaticJoint,
|
||||
|
||||
841
examples/Importers/ImportURDFDemo/UrdfParser.cpp
Normal file
841
examples/Importers/ImportURDFDemo/UrdfParser.cpp
Normal file
@@ -0,0 +1,841 @@
|
||||
#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");
|
||||
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
167
examples/Importers/ImportURDFDemo/UrdfParser.h
Normal 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
17
examples/Importers/ImportURDFDemo/urdfLexicalCast.h
Normal 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
250
examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
Normal 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
30
examples/Importers/ImportURDFDemo/urdfStringSplit.h
Normal 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
|
||||
|
||||
@@ -364,7 +364,7 @@ void PhysicsClient::createClientCommand()
|
||||
void PhysicsClient::stepSimulation(float deltaTime)
|
||||
{
|
||||
|
||||
btAssert(m_testBlock1);
|
||||
// btAssert(m_testBlock1);
|
||||
|
||||
if (m_testBlock1)
|
||||
{
|
||||
|
||||
@@ -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)
|
||||
{
|
||||
|
||||
@@ -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"))
|
||||
|
||||
Reference in New Issue
Block a user