Expose a better API to allow any render engine to be used for the physics simulation when loading URDF/SDF files.
See bullet3/examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.h Give the kuka_iiwa/model.urdf some blue color, not just orange, to mimick the original a bit better Preparation for the CMD_CAMERA_IMAGE_COMPLETED command, to expose a virtual camera to the robotics API
This commit is contained in:
erwin coumans
@@ -37,8 +37,10 @@ struct BulletURDFInternalData
|
||||
UrdfParser m_urdfParser;
|
||||
struct GUIHelperInterface* m_guiHelper;
|
||||
char m_pathPrefix[1024];
|
||||
btHashMap<btHashInt,btVector4> m_linkColors;
|
||||
|
||||
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
||||
|
||||
LinkVisualShapesConverter* m_customVisualShapesConverter;
|
||||
};
|
||||
|
||||
|
||||
@@ -57,13 +59,13 @@ enum MyFileType
|
||||
|
||||
|
||||
|
||||
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper)
|
||||
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
|
||||
{
|
||||
m_data = new BulletURDFInternalData;
|
||||
|
||||
m_data->m_guiHelper = helper;
|
||||
m_data->m_pathPrefix[0]=0;
|
||||
|
||||
m_data->m_customVisualShapesConverter = customConverter;
|
||||
|
||||
|
||||
}
|
||||
@@ -98,7 +100,6 @@ struct BulletErrorLogger : public ErrorLogger
|
||||
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
||||
{
|
||||
|
||||
m_data->m_linkColors.clear();
|
||||
|
||||
|
||||
//int argc=0;
|
||||
@@ -153,9 +154,6 @@ void BulletURDFImporter::activateModel(int modelIndex)
|
||||
bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
|
||||
{
|
||||
|
||||
m_data->m_linkColors.clear();
|
||||
|
||||
|
||||
//int argc=0;
|
||||
char relativeFileName[1024];
|
||||
|
||||
@@ -236,11 +234,9 @@ void BulletURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray
|
||||
|
||||
bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
|
||||
{
|
||||
btVector4* rgbaPtr = m_data->m_linkColors[linkIndex];
|
||||
if (rgbaPtr)
|
||||
if (m_data->m_customVisualShapesConverter)
|
||||
{
|
||||
colorRGBA = *rgbaPtr;
|
||||
return true;
|
||||
return m_data->m_customVisualShapesConverter->getLinkColor(linkIndex, colorRGBA);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@@ -339,327 +335,6 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
|
||||
return true;
|
||||
}
|
||||
|
||||
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:
|
||||
{
|
||||
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:
|
||||
{
|
||||
|
||||
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:
|
||||
{
|
||||
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())
|
||||
{
|
||||
//b3Printf("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();
|
||||
//b3Printf("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:
|
||||
{
|
||||
b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
|
||||
{
|
||||
//apply the geometry scaling
|
||||
for (int i=0;i<glmesh->m_vertices->size();i++)
|
||||
{
|
||||
glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
|
||||
glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
|
||||
glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Warning("mesh geometry not found %s\n", fullPath);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Warning("Error: unknown visual geometry type\n");
|
||||
}
|
||||
}
|
||||
|
||||
//if we have a convex, tesselate into localVertices/localIndices
|
||||
if ((glmesh==0) && 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 hull;
|
||||
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);
|
||||
}
|
||||
}
|
||||
delete glmesh;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
|
||||
@@ -910,61 +585,17 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
return shape;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
|
||||
int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionShape* colShape) 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)
|
||||
{
|
||||
int graphicsIndex = -1;
|
||||
|
||||
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;
|
||||
btHashString matName(vis.m_materialName.c_str());
|
||||
UrdfMaterial *const * matPtr = model.m_materials[matName];
|
||||
if (matPtr)
|
||||
{
|
||||
UrdfMaterial *const mat = *matPtr;
|
||||
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
|
||||
m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
|
||||
}
|
||||
convertURDFToVisualShape(&vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
|
||||
|
||||
|
||||
}
|
||||
if (m_data->m_customVisualShapesConverter)
|
||||
{
|
||||
const UrdfModel& model = m_data->m_urdfParser.getModel();
|
||||
graphicsIndex = m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colShape);
|
||||
}
|
||||
#endif
|
||||
if (vertices.size() && indices.size())
|
||||
{
|
||||
graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
|
||||
}
|
||||
|
||||
return graphicsIndex;
|
||||
|
||||
|
||||
}
|
||||
|
||||
int BulletURDFImporter::getNumAllocatedCollisionShapes() const
|
||||
@@ -985,25 +616,6 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
|
||||
m_data->m_allocatedCollisionShapes.push_back(compoundShape);
|
||||
|
||||
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)
|
||||
@@ -1025,8 +637,6 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
return compoundShape;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user