From b64f5feba4d805a7bcefb02583380b07abb3c909 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Wed, 22 Apr 2015 16:35:27 -0700 Subject: [PATCH] refactor of URDF importer (work-in-progress) --- .../ImportURDFDemo/ImportURDFSetup.cpp | 1374 +---------------- .../MultiBodyCreationInterface.h | 29 + .../ImportURDFDemo/MyMultiBodyCreator.cpp | 71 + .../ImportURDFDemo/MyMultiBodyCreator.h | 48 + .../ImportURDFDemo/MyURDFImporter.cpp | 762 +++++++++ .../Importers/ImportURDFDemo/MyURDFImporter.h | 41 + .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 57 +- .../Importers/ImportURDFDemo/URDF2Bullet.h | 61 +- .../ImportURDFDemo/URDFImporterInterface.h | 41 + .../Importers/ImportURDFDemo/URDFJointTypes.h | 16 + 10 files changed, 1093 insertions(+), 1407 deletions(-) create mode 100644 examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h create mode 100644 examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp create mode 100644 examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h create mode 100644 examples/Importers/ImportURDFDemo/MyURDFImporter.cpp create mode 100644 examples/Importers/ImportURDFDemo/MyURDFImporter.h create mode 100644 examples/Importers/ImportURDFDemo/URDFImporterInterface.h create mode 100644 examples/Importers/ImportURDFDemo/URDFJointTypes.h diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index a7373a649..0cc64e3d2 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -1,16 +1,15 @@ #include "ImportURDFSetup.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" -#include "Bullet3Common/b3FileUtils.h" -#include "../ImportObjDemo/LoadMeshFromObj.h" -#include "../ImportSTLDemo/LoadMeshFromSTL.h" -#include "../ImportColladaDemo/LoadMeshFromCollada.h" + + #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "Bullet3Common/b3FileUtils.h" -#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape + #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "../CommonInterfaces/CommonParameterInterface.h" +#include "MyURDFImporter.h" static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter; static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter); @@ -32,6 +31,7 @@ using namespace urdf; #include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "MyMultiBodyCreator.h" class ImportUrdfSetup : public CommonMultiBodyBase @@ -51,251 +51,6 @@ public: void setFileName(const char* urdfFileName); }; -void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut); -btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix); - -class MyURDF2Bullet : public URDF2Bullet -{ - my_shared_ptr m_robot; - std::vector > m_links; - - mutable btMultiBody* m_bulletMultiBody; - - struct GUIHelperInterface* m_guiHelper; - -public: - - mutable btAlignedObjectArray m_urdf2mbLink; - mutable btAlignedObjectArray m_mb2urdfLink; - - - MyURDF2Bullet(my_shared_ptr robot,struct GUIHelperInterface* helper) - :m_robot(robot), - m_guiHelper(helper), - m_bulletMultiBody(0) - { - m_robot->getLinks(m_links); - - //initialize the 'index' of each link - for (int i=0;im_link_index = i; - } - - m_urdf2mbLink.resize(m_links.size(),-2); - m_mb2urdfLink.resize(m_links.size(),-2); - } - - virtual int getRootLinkIndex() const - { - if (m_links.size()) - { - int rootLinkIndex = m_robot->getRoot()->m_link_index; - // btAssert(m_links[0]->m_link_index == rootLinkIndex); - return rootLinkIndex; - } - return -1; - }; - - virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const - { - childLinkIndices.resize(0); - int numChildren = m_links[linkIndex]->child_links.size(); - - for (int i=0;ichild_links[i]->m_link_index; - childLinkIndices.push_back(childIndex); - } - } - virtual std::string getLinkName(int linkIndex) const - { - std::string n = m_links[linkIndex]->name; - return n; - } - - virtual std::string getJointName(int linkIndex) const - { - return m_links[linkIndex]->parent_joint->name; - } - - virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const - { - if ((*m_links[linkIndex]).inertial) - { - mass = (*m_links[linkIndex]).inertial->mass; - localInertiaDiagonal.setValue((*m_links[linkIndex]).inertial->ixx,(*m_links[linkIndex]).inertial->iyy,(*m_links[linkIndex]).inertial->izz); - inertialFrame.setOrigin(btVector3((*m_links[linkIndex]).inertial->origin.position.x,(*m_links[linkIndex]).inertial->origin.position.y,(*m_links[linkIndex]).inertial->origin.position.z)); - inertialFrame.setRotation(btQuaternion((*m_links[linkIndex]).inertial->origin.rotation.x,(*m_links[linkIndex]).inertial->origin.rotation.y,(*m_links[linkIndex]).inertial->origin.rotation.z,(*m_links[linkIndex]).inertial->origin.rotation.w)); - } else - { - mass = 1.f; - localInertiaDiagonal.setValue(1,1,1); - inertialFrame.setIdentity(); - } - } - - virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const - { - jointLowerLimit = 0.f; - jointUpperLimit = 0.f; - - if ((*m_links[urdfLinkIndex]).parent_joint) - { - my_shared_ptr pj =(*m_links[urdfLinkIndex]).parent_joint; - - const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position; - const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation; - - jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z); - parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z)); - parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w)); - - switch (pj->type) - { - case Joint::REVOLUTE: - jointType = URDF2Bullet::RevoluteJoint; - break; - case Joint::FIXED: - jointType = URDF2Bullet::FixedJoint; - break; - case Joint::PRISMATIC: - jointType = URDF2Bullet::PrismaticJoint; - break; - case Joint::PLANAR: - jointType = URDF2Bullet::PlanarJoint; - break; - case Joint::CONTINUOUS: - jointType = URDF2Bullet::ContinuousJoint; - break; - default: - { - printf("Error: unknown joint type %d\n", pj->type); - btAssert(0); - } - - }; - - if (pj->limits) - { - jointLowerLimit = pj->limits.get()->lower; - jointUpperLimit = pj->limits.get()->upper; - } - return true; - } else - { - parent2joint.setIdentity(); - return false; - } - } - - virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const - { - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - btTransform startTrans; startTrans.setIdentity(); - int graphicsIndex = -1; - - for (int v = 0; v < (int)m_links[linkIndex]->visual_array.size(); v++) - { - const Visual* vis = 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); - - } - - if (vertices.size() && indices.size()) - { - graphicsIndex = m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); - } - - return graphicsIndex; - - } - - virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const - { - - btCompoundShape* compoundShape = new btCompoundShape(); - compoundShape->setMargin(0.001); - - for (int v=0;v<(int)m_links[linkIndex]->collision_array.size();v++) - { - const Collision* col = 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); - - } - } - - return compoundShape; - } - - virtual class btMultiBody* allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) const - { - m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof); - return m_bulletMultiBody; - } - - virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) const - { - btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal); - rbci.m_startWorldTransform = initialWorldTrans; - btRigidBody* body = new btRigidBody(rbci); - return body; - } - - virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) const - { - btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex); - return mbCol; - } - - - virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) const - { - btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder); - return c; - } - - virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) const - { - m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex; - m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex; - } - - virtual void createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const - { - - m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba); - } - - virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba) const - { - m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba); - } - - btMultiBody* getBulletMultiBody() - { - return m_bulletMultiBody; - } - -}; - - btAlignedObjectArray gFileNameArray; @@ -394,7 +149,7 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName) -void printTree(my_shared_ptr link,int level = 0) +void printTree1(my_shared_ptr link,int level = 0) { level+=2; int count = 0; @@ -405,7 +160,7 @@ void printTree(my_shared_ptr link,int level = 0) for(int j=0;jname << std::endl; // first grandchild - printTree(*child,level); + printTree1(*child,level); } else { @@ -471,1032 +226,9 @@ struct URDF2BulletMappings } }; -enum MyFileType -{ - FILE_STL=1, - FILE_COLLADA=2, - FILE_OBJ=3, -}; -void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) -{ - - - GLInstanceGraphicsShape* glmesh = 0; - - btConvexShape* convexColShape = 0; - - switch (visual->geometry->type) - { - case Geometry::CYLINDER: - { - printf("processing a cylinder\n"); - urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); - btAlignedObjectArray vertices; - - //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); - int numSteps = 32; - for (int i = 0; iradius*btSin(SIMD_2_PI*(float(i) / numSteps)), cyl->radius*btCos(SIMD_2_PI*(float(i) / numSteps)), cyl->length / 2.); - vertices.push_back(vert); - vert[2] = -cyl->length / 2.; - vertices.push_back(vert); - } - - btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - cylZShape->setMargin(0.001); - convexColShape = cylZShape; - break; - } - case Geometry::BOX: - { - printf("processing a box\n"); - urdf::Box* box = (urdf::Box*)visual->geometry.get(); - btVector3 extents(box->dim.x, box->dim.y, box->dim.z); - 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 Geometry::SPHERE: - { - printf("processing a sphere\n"); - urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); - btScalar radius = sphere->radius; - btSphereShape* sphereShape = new btSphereShape(radius); - convexColShape = sphereShape; - convexColShape->setMargin(0.001); - break; - - break; - } - case Geometry::MESH: - { - if (visual->name.length()) - { - printf("visual->name=%s\n", visual->name.c_str()); - } - if (visual->geometry) - { - const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get(); - if (mesh->filename.length()) - { - const char* filename = mesh->filename.c_str(); - printf("mesh->filename=%s\n", filename); - char fullPath[1024]; - int fileType = 0; - sprintf(fullPath, "%s%s", pathPrefix, 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", pathPrefix, filename); - FILE* f = fopen(fullPath, "rb"); - if (f) - { - fclose(f); - - - - switch (fileType) - { - case FILE_OBJ: - { - glmesh = LoadMeshFromObj(fullPath,pathPrefix); - break; - } - - case FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; - } - case FILE_COLLADA: - { - - btAlignedObjectArray visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans; upAxisTrans.setIdentity(); - float unitMeterScaling = 1; - int upAxis = 2; - - LoadMeshFromCollada(fullPath, - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis); - - glmesh = new GLInstanceGraphicsShape; - int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i = 0; im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i = 0; im_vertices->size(); i++) - { - verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; - verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; - verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; - verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; - verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; - verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; - verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; - verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; - verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - - } - - int curNumIndices = glmesh->m_indices->size(); - int additionalIndices = gfxShape->m_indices->size(); - glmesh->m_indices->resize(curNumIndices + additionalIndices); - for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setIdentity(); -// upAxisMat.setPureRotation(upAxisTrans.getRotation()); - btMatrix4x4 unitMeterScalingMat; - unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; - //btMatrix4x4 worldMat = instance->m_worldTransform; - int curNumVertices = glmesh->m_vertices->size(); - int additionalVertices = verts.size(); - glmesh->m_vertices->reserve(curNumVertices + additionalVertices); - - for (int v = 0; vm_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(fullPath); - - break; - } - default: - { - printf("Error: unsupported file type for Visual mesh: %s\n", fullPath); - btAssert(0); - } - } - - - if (glmesh && (glmesh->m_numvertices>0)) - { - } - else - { - printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath); - } - - } - else - { - printf("mesh geometry not found %s\n", fullPath); - } - - - } - } - - - break; - } - default: - { - printf("Error: unknown visual geometry type\n"); - } - } - - //if we have a convex, tesselate into localVertices/localIndices - if (convexColShape) - { - btShapeHull* hull = new btShapeHull(convexColShape); - hull->buildHull(0.0); - { - // int strideInBytes = 9*sizeof(float); - int numVertices = hull->numVertices(); - int numIndices = hull->numIndices(); - - - glmesh = new GLInstanceGraphicsShape; - int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - - for (int i = 0; i < numVertices; i++) - { - GLInstanceVertex vtx; - btVector3 pos = hull->getVertexPointer()[i]; - vtx.xyzw[0] = pos.x(); - vtx.xyzw[1] = pos.y(); - vtx.xyzw[2] = pos.z(); - vtx.xyzw[3] = 1.f; - pos.normalize(); - vtx.normal[0] = pos.x(); - vtx.normal[1] = pos.y(); - vtx.normal[2] = pos.z(); - vtx.uv[0] = 0.5f; - vtx.uv[1] = 0.5f; - glmesh->m_vertices->push_back(vtx); - } - - btAlignedObjectArray indices; - for (int i = 0; i < numIndices; i++) - { - glmesh->m_indices->push_back(hull->getIndexPointer()[i]); - } - - glmesh->m_numvertices = glmesh->m_vertices->size(); - glmesh->m_numIndices = glmesh->m_indices->size(); - } - delete convexColShape; - convexColShape = 0; - - } - - if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0) - { - - int baseIndex = verticesOut.size(); - - - - for (int i = 0; i < glmesh->m_indices->size(); i++) - { - indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex); - } - - for (int i = 0; i < glmesh->m_vertices->size(); i++) - { - GLInstanceVertex& v = glmesh->m_vertices->at(i); - btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]); - btVector3 vt = visualTransform*vert; - v.xyzw[0] = vt[0]; - v.xyzw[1] = vt[1]; - v.xyzw[2] = vt[2]; - btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]); - triNormal = visualTransform.getBasis()*triNormal; - v.normal[0] = triNormal[0]; - v.normal[1] = triNormal[1]; - v.normal[2] = triNormal[2]; - verticesOut.push_back(v); - } - } -} - -btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix) -{ - btCollisionShape* shape = 0; - - switch (visual->geometry->type) - { - case Geometry::CYLINDER: - { - printf("processing a cylinder\n"); - urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); - - btAlignedObjectArray vertices; - //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); - int numSteps = 32; - for (int i=0;iradius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.); - vertices.push_back(vert); - vert[2] = -cyl->length/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 Geometry::BOX: - { - printf("processing a box\n"); - urdf::Box* box = (urdf::Box*)visual->geometry.get(); - btVector3 extents(box->dim.x,box->dim.y,box->dim.z); - 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 Geometry::SPHERE: - { - printf("processing a sphere\n"); - urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); - btScalar radius = sphere->radius; - btSphereShape* sphereShape = new btSphereShape(radius); - shape = sphereShape; - shape ->setMargin(0.001); - break; - - break; - } - case Geometry::MESH: - { - if (visual->name.length()) - { - printf("visual->name=%s\n",visual->name.c_str()); - } - if (visual->geometry) - { - const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get(); - if (mesh->filename.length()) - { - const char* filename = mesh->filename.c_str(); - printf("mesh->filename=%s\n",filename); - char fullPath[1024]; - int fileType = 0; - sprintf(fullPath,"%s%s",pathPrefix,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",pathPrefix,filename); - FILE* f = fopen(fullPath,"rb"); - if (f) - { - fclose(f); - GLInstanceGraphicsShape* glmesh = 0; - - - switch (fileType) - { - case FILE_OBJ: - { - glmesh = LoadMeshFromObj(fullPath,pathPrefix); - break; - } - case FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; - } - case FILE_COLLADA: - { - - btAlignedObjectArray visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans;upAxisTrans.setIdentity(); - float unitMeterScaling=1; - int upAxis = 2; - LoadMeshFromCollada(fullPath, - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis ); - - glmesh = new GLInstanceGraphicsShape; - int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i=0;im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i=0;im_vertices->size();i++) - { - verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; - verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; - verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; - verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; - verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; - verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; - verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; - verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; - verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - - } - - int curNumIndices = glmesh->m_indices->size(); - int additionalIndices = gfxShape->m_indices->size(); - glmesh->m_indices->resize(curNumIndices+additionalIndices); - for (int k=0;km_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setPureRotation(upAxisTrans.getRotation()); - btMatrix4x4 unitMeterScalingMat; - unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat; - //btMatrix4x4 worldMat = instance->m_worldTransform; - int curNumVertices = glmesh->m_vertices->size(); - int additionalVertices = verts.size(); - glmesh->m_vertices->reserve(curNumVertices+additionalVertices); - - for(int v=0;vm_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(fullPath); - - break; - } - default: - { - printf("Unsupported file type in Collision: %s\n",fullPath); - btAssert(0); - } - } - - - if (glmesh && (glmesh->m_numvertices>0)) - { - printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); - //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); - //convex->setUserIndex(shapeId); - btAlignedObjectArray convertedVerts; - convertedVerts.reserve(glmesh->m_numvertices); - for (int i=0;im_numvertices;i++) - { - convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2])); - } - //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex)); - btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); - //cylZShape->initializePolyhedralFeatures(); - //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); - //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); - cylZShape->setMargin(0.001); - shape = cylZShape; - } else - { - printf("issue extracting mesh from STL file %s\n", fullPath); - } - - } else - { - printf("mesh geometry not found %s\n",fullPath); - } - - - } - } - - - break; - } - default: - { - printf("Error: unknown visual geometry type\n"); - } - } - return shape; -} -void URDFvisual2BulletCollisionShape(my_shared_ptr link, struct GUIHelperInterface* helper, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix) -{ - //btCollisionShape* shape = 0; - - btTransform linkTransformInWorldSpace; - linkTransformInWorldSpace.setIdentity(); - - btScalar mass = 0; - btTransform inertialFrame; - inertialFrame.setIdentity(); - const Link* parentLink = (*link).getParent(); - URDF_LinkInformation* pp = 0; - - int linkIndex = mappings.m_linkMasses.size();//assuming root == 0, child links use contiguous numbering > 0 - - btVector3 localInertiaDiagonal(0,0,0); - - int parentIndex = -1; - - - if (parentLink) - { - parentIndex = mappings.m_urdfLinkIndices2BulletLinkIndices[parentLink->m_link_index]; - - btAssert(parentIndex>=0); - } - - { - URDF_LinkInformation** ppRigidBody = mappings.m_link2rigidbody.find(parentLink); - if (ppRigidBody) - { - pp = (*ppRigidBody); - btTransform tr = pp->m_bodyWorldTransform; - printf("rigidbody origin (COM) of link(%s) parent(%s): %f,%f,%f\n",(*link).name.c_str(), parentLink->name.c_str(), tr.getOrigin().x(), tr.getOrigin().y(), tr.getOrigin().z()); - } - } - - mappings.m_urdfLinkIndices2BulletLinkIndices[(*link).m_link_index] = linkIndex; - - if ((*link).inertial) - { - mass = (*link).inertial->mass; - localInertiaDiagonal.setValue((*link).inertial->ixx,(*link).inertial->iyy,(*link).inertial->izz); - inertialFrame.setOrigin(btVector3((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z)); - inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w)); - } - - - btTransform parent2joint; - parent2joint.setIdentity(); - - if ((*link).parent_joint) - { - - const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position; - const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation; - - parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z)); - parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w)); - linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; - } else - { - linkTransformInWorldSpace = parentTransformInWorldSpace; - - - - } - - - { - printf("converting visuals of link %s", link->name.c_str()); - - - - { - - - - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - btTransform startTrans; startTrans.setIdentity(); - int graphicsIndex = -1; - - for (int v = 0; v < (int)link->visual_array.size(); v++) - { - const Visual* vis = link->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); - - } - - if (vertices.size() && indices.size()) - { - graphicsIndex = helper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); - } - - - - btCompoundShape* compoundShape = new btCompoundShape(); - compoundShape->setMargin(0.001); - for (int v=0;v<(int)link->collision_array.size();v++) - { - const Collision* col = link->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(inertialFrame.inverse()*childTrans,childShape); - - } - } - - - - - - if (compoundShape) - { - - - btVector3 color = selectColor(); - /* if (visual->material.get()) - { - color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); - } - */ - //btVector3 localInertiaDiagonal(0, 0, 0); - //if (mass) - //{ - // shape->calculateLocalInertia(mass, localInertiaDiagonal); - //} - - - //btTransform visualFrameInWorldSpace = linkTransformInWorldSpace*visual_frame; - btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*inertialFrame; - URDF_LinkInformation* linkInfo = new URDF_LinkInformation; - - if (!mappings.m_createMultiBody) - { - btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal); - rbci.m_startWorldTransform = inertialFrameInWorldSpace; - linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace - //rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass; - btRigidBody* body = new btRigidBody(rbci); - world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask); - - compoundShape->setUserIndex(graphicsIndex); - - helper->createRigidBodyGraphicsObject(body, color); - linkInfo->m_bulletRigidBody = body; - } else - { - if (mappings.m_bulletMultiBody==0) - { - bool multiDof = true; - bool canSleep = false; - bool isFixedBase = (mass==0);//todo: figure out when base is fixed - int totalNumJoints = mappings.m_totalNumJoints; - mappings.m_bulletMultiBody = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); - } - - } - - - linkInfo->m_collisionShape = compoundShape; - linkInfo->m_localInertiaDiagonal = localInertiaDiagonal; - linkInfo->m_mass = mass; - //linkInfo->m_localVisualFrame =visual_frame; - linkInfo->m_localInertialFrame =inertialFrame; - linkInfo->m_thisLink = link.get(); - const Link* p = link.get(); - mappings.m_link2rigidbody.insert(p, linkInfo); - - //create a joint if necessary - if ((*link).parent_joint && pp) - { - btAssert(pp); - - - - const Joint* pj = (*link).parent_joint.get(); - btTransform offsetInA,offsetInB; - static bool once = true; - - offsetInA.setIdentity(); - static bool toggle=false; - - //offsetInA = pp->m_localVisualFrame.inverse()*parent2joint; - offsetInA = pp->m_localInertialFrame.inverse()*parent2joint; - - offsetInB.setIdentity(); - //offsetInB = visual_frame.inverse(); - offsetInB = inertialFrame.inverse(); - - - bool disableParentCollision = true; - btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z); - switch (pj->type) - { - case Joint::FIXED: - { - if (mappings.m_createMultiBody) - { - //todo: adjust the center of mass transform and pivot axis properly - - printf("Fixed joint (btMultiBody)\n"); - //btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin()); - btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation(); - //toggle=!toggle; - //mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - // rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision); - mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision); - - /* - mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(), - -offsetInB.getOrigin(), - disableParentCollision); - */ - - btMatrix3x3 rm(rot); - btScalar y,p,r; - rm.getEulerZYX(y,p,r); - //parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); - //linkInfo->m_localVisualFrame.setIdentity(); - printf("y=%f,p=%f,r=%f\n", y,p,r); - - - - } else - { - printf("Fixed joint\n"); - - btMatrix3x3 rm(offsetInA.getBasis()); - btScalar y,p,r; - rm.getEulerZYX(y,p,r); - //parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); - //linkInfo->m_localVisualFrame.setIdentity(); - printf("y=%f,p=%f,r=%f\n", y,p,r); - - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB); - // btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - - dof6->setAngularLowerLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,0)); - - if (enableConstraints) - world1->addConstraint(dof6,true); - - // btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB); - // world->addConstraint(fixed,true); - } - break; - } - case Joint::CONTINUOUS: - case Joint::REVOLUTE: - { - if (mappings.m_createMultiBody) - { - //todo: adjust the center of mass transform and pivot axis properly - /*mappings.m_bulletMultiBody->setupRevolute( - linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - - parent2joint.inverse().getRotation(), jointAxis, parent2joint.getOrigin(), - btVector3(0,0,0),//offsetInB.getOrigin(), - disableParentCollision); - */ - - - mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - //parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(), - offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(), - - -offsetInB.getOrigin(), - disableParentCollision); - //linkInfo->m_localVisualFrame.setIdentity(); - - } else - { - //only handle principle axis at the moment, - //@todo(erwincoumans) orient the constraint for non-principal axis - btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z); - int principleAxis = axis.closestAxis(); - switch (principleAxis) - { - case 0: - { - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_ZYX); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - - dof6->setAngularUpperLimit(btVector3(-1,0,0)); - dof6->setAngularLowerLimit(btVector3(1,0,0)); - - if (enableConstraints) - world1->addConstraint(dof6,true); - break; - } - case 1: - { - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XZY); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - - dof6->setAngularUpperLimit(btVector3(0,-1,0)); - dof6->setAngularLowerLimit(btVector3(0,1,0)); - - if (enableConstraints) - world1->addConstraint(dof6,true); - break; - } - case 2: - default: - { - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XYZ); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - - dof6->setAngularUpperLimit(btVector3(0,0,-1)); - dof6->setAngularLowerLimit(btVector3(0,0,0)); - - if (enableConstraints) - world1->addConstraint(dof6,true); - } - }; - printf("Revolute/Continuous joint\n"); - } - break; - } - case Joint::PRISMATIC: - { - if (mappings.m_createMultiBody) - { - //mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - // parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision); - - //mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - // parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision); - - mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, - offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(), - -offsetInB.getOrigin(), - disableParentCollision); - - - - } else - { - - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB); - //todo(erwincoumans) for now, we only support principle axis along X, Y or Z - btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z); - int principleAxis = axis.closestAxis(); - switch (principleAxis) - { - case 0: - { - dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0)); - dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0)); - break; - } - case 1: - { - dof6->setLinearLowerLimit(btVector3(0,pj->limits->lower,0)); - dof6->setLinearUpperLimit(btVector3(0,pj->limits->upper,0)); - break; - } - case 2: - default: - { - dof6->setLinearLowerLimit(btVector3(0,0,pj->limits->lower)); - dof6->setLinearUpperLimit(btVector3(0,0,pj->limits->upper)); - } - }; - - dof6->setAngularLowerLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,0)); - if (enableConstraints) - world1->addConstraint(dof6,true); - - printf("Prismatic\n"); - } - break; - } - default: - { - printf("Error: unsupported joint type in URDF (%d)\n", pj->type); - } - } - - } - - if (mappings.m_createMultiBody) - { - if (compoundShape->getNumChildShapes()>0) - { - btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mappings.m_bulletMultiBody, linkIndex-1); - - //btCompoundShape* comp = new btCompoundShape(); - //comp->addChildShape(linkInfo->m_localVisualFrame,shape); - - compoundShape->setUserIndex(graphicsIndex); - - col->setCollisionShape(compoundShape); - - btTransform tr; - tr.setIdentity(); - tr = linkTransformInWorldSpace; - //if we don't set the initial pose of the btCollisionObject, the simulator will do this - //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider - - //tr.setOrigin(local_origin[0]); - //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - col->setWorldTransform(tr); - - bool isDynamic = true; - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask); - - btVector3 color = selectColor();//(0.0,0.0,0.5); - - helper->createCollisionObjectGraphicsObject(col,color); - btScalar friction = 0.5f; - - col->setFriction(friction); - - if (parentIndex>=0) - { - mappings.m_bulletMultiBody->getLink(linkIndex-1).m_collider=col; - } else - { - mappings.m_bulletMultiBody->setBaseCollider(col); - } - } - } - - //mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal); - //mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform); - } - - - } - } - - mappings.m_linkMasses.push_back(mass); - - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - URDFvisual2BulletCollisionShape(*child,helper, linkTransformInWorldSpace, world1,mappings,pathPrefix); - - } - else - { - std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl; - } - } - - - - -} - - void ImportUrdfSetup::initPhysics() { @@ -1567,13 +299,13 @@ void ImportUrdfSetup::initPhysics() std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; // print entire tree - printTree(root_link); + printTree1(root_link); printf("now using new interface\n"); std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; //now print the tree using the new interface - MyURDF2Bullet u2b(robot,m_guiHelper); - printTree(u2b, 0,0); + MyURDFImporter u2b(robot,m_guiHelper); + printTree(u2b, 0); btTransform identityTrans; identityTrans.setIdentity(); @@ -1588,70 +320,56 @@ void ImportUrdfSetup::initPhysics() btMultiBody* mb = 0; - if (!useUrdfInterfaceClass) - { - mappings.m_createMultiBody = m_useMultiBody; - mappings.m_totalNumJoints = numJoints; - mappings.m_urdfLinkIndices2BulletLinkIndices.resize(numJoints+1,-2);//root and child links (=1+numJoints) - URDFvisual2BulletCollisionShape(root_link, m_guiHelper, identityTrans,m_dynamicsWorld,mappings,pathPrefix); - mb = mappings.m_bulletMultiBody; - if (m_useMultiBody) - { - mb->setHasSelfCollision(false); - mb->finalizeMultiDof(); - m_dynamicsWorld->addMultiBody(mb); - } - } else - { + - //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user - int rootLinkIndex = u2b.getRootLinkIndex(); - printf("urdf root link index = %d\n",rootLinkIndex); - ConvertURDF2Bullet(u2b,identityTrans,m_dynamicsWorld,m_useMultiBody,pathPrefix); - mb = u2b.getBulletMultiBody(); + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user + int rootLinkIndex = u2b.getRootLinkIndex(); + printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_guiHelper); - if (m_useMultiBody) - { - mb->setHasSelfCollision(false); - mb->finalizeMultiDof(); - m_dynamicsWorld->addMultiBody(mb); + ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,pathPrefix); + mb = creation.getBulletMultiBody(); + + if (m_useMultiBody) + { + mb->setHasSelfCollision(false); + mb->finalizeMultiDof(); + m_dynamicsWorld->addMultiBody(mb); - //create motors for each joint + //create motors for each joint - for (int i=0;igetNumLinks();i++) + for (int i=0;igetNumLinks();i++) + { + int mbLinkIndex = i; + if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute) { - int mbLinkIndex = i; - if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute) + if (m_data->m_numMotorsm_numMotorsm_motorTargetVelocities[m_data->m_numMotors]; - *motorVel = 0.f; - SliderParams slider(motorName,motorVel); - slider.m_minVal=-4; - slider.m_maxVal=4; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - float maxMotorImpulse = 0.1f; - btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); - m_data->m_jointMotors[m_data->m_numMotors]=motor; - m_dynamicsWorld->addMultiBodyConstraint(motor); - m_data->m_numMotors++; - } + std::string jointName = u2b.getJointName(urdfLinkIndex); + char motorName[1024]; + sprintf(motorName,"%s q'", jointName.c_str()); + btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors]; + *motorVel = 0.f; + SliderParams slider(motorName,motorVel); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + float maxMotorImpulse = 0.1f; + btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); + m_data->m_jointMotors[m_data->m_numMotors]=motor; + m_dynamicsWorld->addMultiBodyConstraint(motor); + m_data->m_numMotors++; } - } + } } - } - + //the btMultiBody support is work-in-progress :-) diff --git a/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h new file mode 100644 index 000000000..293290c22 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h @@ -0,0 +1,29 @@ +#ifndef MULTIBODY_CREATION_INTERFACE_H +#define MULTIBODY_CREATION_INTERFACE_H + +#include "LinearMath/btTransform.h" + +class MultiBodyCreationInterface +{ +public: + + virtual ~MultiBodyCreationInterface() {} + + + virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 0; + + ///optionally create some graphical representation from a collision object, usually for visual debugging purposes. + virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0; + + virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) =0; + + virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0; + + virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) = 0; + + virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0; + + virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0; +}; + +#endif //MULTIBODY_CREATION_INTERFACE_H \ No newline at end of file diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp new file mode 100644 index 000000000..fb2ca9dcb --- /dev/null +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -0,0 +1,71 @@ +#include "MyMultiBodyCreator.h" + +#include "../CommonInterfaces/CommonGUIHelperInterface.h" + +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" + +#include "BulletCollision/CollisionShapes/btCompoundShape.h" + +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" + + +MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper) + :m_guiHelper(guiHelper), + m_bulletMultiBody(0) +{ +} + + + class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) +{ + m_urdf2mbLink.resize(totalNumJoints+1,-2); + m_mb2urdfLink.resize(totalNumJoints+1,-2); + + m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof); + return m_bulletMultiBody; +} + +class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) +{ + btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal); + rbci.m_startWorldTransform = initialWorldTrans; + btRigidBody* body = new btRigidBody(rbci); + return body; +} + +class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) +{ + btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex); + return mbCol; +} + + +class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) +{ + btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder); + return c; +} + +void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex) +{ + m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex; + m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex; +} + +void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) +{ + + m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba); +} + +void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba) +{ + m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba); +} + +btMultiBody* MyMultiBodyCreator::getBulletMultiBody() +{ + return m_bulletMultiBody; +} diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h new file mode 100644 index 000000000..1a5df1b9c --- /dev/null +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h @@ -0,0 +1,48 @@ + +#ifndef MY_MULTIBODY_CREATOR +#define MY_MULTIBODY_CREATOR + +#include "MultiBodyCreationInterface.h" +#include "LinearMath/btAlignedObjectArray.h" + +class GUIHelperInterface; +class btMultiBody; + +class MyMultiBodyCreator : public MultiBodyCreationInterface +{ + + btMultiBody* m_bulletMultiBody; + + struct GUIHelperInterface* m_guiHelper; + + + +public: + + btAlignedObjectArray m_urdf2mbLink; + btAlignedObjectArray m_mb2urdfLink; + + + MyMultiBodyCreator(GUIHelperInterface* guiHelper); + + virtual ~MyMultiBodyCreator() {} + + virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) ; + + ///optionally create some graphical representation from a collision object, usually for visual debugging purposes. + virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba); + + virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof); + + virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape); + + virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0); + + virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body); + + virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex); + + btMultiBody* getBulletMultiBody(); +}; + +#endif //MY_MULTIBODY_CREATOR diff --git a/examples/Importers/ImportURDFDemo/MyURDFImporter.cpp b/examples/Importers/ImportURDFDemo/MyURDFImporter.cpp new file mode 100644 index 000000000..d3494ef0c --- /dev/null +++ b/examples/Importers/ImportURDFDemo/MyURDFImporter.cpp @@ -0,0 +1,762 @@ +#include "MyURDFImporter.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" + +using namespace urdf; + +void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut); +btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix); + + +struct MyURDFInternalData +{ + my_shared_ptr m_robot; + std::vector > m_links; + struct GUIHelperInterface* m_guiHelper; + +}; + +enum MyFileType +{ + FILE_STL=1, + FILE_COLLADA=2, + FILE_OBJ=3, +}; + + + +MyURDFImporter::MyURDFImporter(my_shared_ptr robot,struct GUIHelperInterface* helper) +{ + m_data = new MyURDFInternalData; + m_data->m_robot = robot; + m_data->m_guiHelper = helper; + m_data->m_robot->getLinks(m_data->m_links); + + //initialize the 'index' of each link + for (int i=0;im_links.size();i++) + { + m_data->m_links[i]->m_link_index = i; + } + +} + +MyURDFImporter::~MyURDFImporter() +{ + delete m_data; +} + + +int MyURDFImporter::getRootLinkIndex() const +{ + if (m_data->m_links.size()) + { + int rootLinkIndex = m_data->m_robot->getRoot()->m_link_index; + // btAssert(m_links[0]->m_link_index == rootLinkIndex); + return rootLinkIndex; + } + return -1; +}; + +void MyURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const +{ + childLinkIndices.resize(0); + int numChildren = m_data->m_links[linkIndex]->child_links.size(); + + for (int i=0;im_links[linkIndex]->child_links[i]->m_link_index; + childLinkIndices.push_back(childIndex); + } +} + +std::string MyURDFImporter::getLinkName(int linkIndex) const +{ + std::string n = m_data->m_links[linkIndex]->name; + return n; +} + +std::string MyURDFImporter::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 +{ + if ((*m_data->m_links[linkIndex]).inertial) + { + mass = (*m_data->m_links[linkIndex]).inertial->mass; + localInertiaDiagonal.setValue((*m_data->m_links[linkIndex]).inertial->ixx,(*m_data->m_links[linkIndex]).inertial->iyy,(*m_data->m_links[linkIndex]).inertial->izz); + inertialFrame.setOrigin(btVector3((*m_data->m_links[linkIndex]).inertial->origin.position.x,(*m_data->m_links[linkIndex]).inertial->origin.position.y,(*m_data->m_links[linkIndex]).inertial->origin.position.z)); + inertialFrame.setRotation(btQuaternion((*m_data->m_links[linkIndex]).inertial->origin.rotation.x,(*m_data->m_links[linkIndex]).inertial->origin.rotation.y,(*m_data->m_links[linkIndex]).inertial->origin.rotation.z,(*m_data->m_links[linkIndex]).inertial->origin.rotation.w)); + } else + { + mass = 1.f; + localInertiaDiagonal.setValue(1,1,1); + inertialFrame.setIdentity(); + } +} + +bool MyURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const +{ + jointLowerLimit = 0.f; + jointUpperLimit = 0.f; + + if ((*m_data->m_links[urdfLinkIndex]).parent_joint) + { + my_shared_ptr pj =(*m_data->m_links[urdfLinkIndex]).parent_joint; + + const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position; + const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation; + + jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z); + parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z)); + parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w)); + + switch (pj->type) + { + case Joint::REVOLUTE: + jointType = URDFRevoluteJoint; + break; + case Joint::FIXED: + jointType = URDFFixedJoint; + break; + case Joint::PRISMATIC: + jointType = URDFPrismaticJoint; + break; + case Joint::PLANAR: + jointType = URDFPlanarJoint; + break; + case Joint::CONTINUOUS: + jointType = URDFContinuousJoint; + break; + default: + { + printf("Error: unknown joint type %d\n", pj->type); + btAssert(0); + } + + }; + + if (pj->limits) + { + jointLowerLimit = pj->limits.get()->lower; + jointUpperLimit = pj->limits.get()->upper; + } + return true; + } else + { + parent2joint.setIdentity(); + return false; + } +} + + + +void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) +{ + + + GLInstanceGraphicsShape* glmesh = 0; + + btConvexShape* convexColShape = 0; + + switch (visual->geometry->type) + { + case Geometry::CYLINDER: + { + printf("processing a cylinder\n"); + urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); + btAlignedObjectArray vertices; + + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i = 0; iradius*btSin(SIMD_2_PI*(float(i) / numSteps)), cyl->radius*btCos(SIMD_2_PI*(float(i) / numSteps)), cyl->length / 2.); + vertices.push_back(vert); + vert[2] = -cyl->length / 2.; + vertices.push_back(vert); + } + + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->setMargin(0.001); + convexColShape = cylZShape; + break; + } + case Geometry::BOX: + { + printf("processing a box\n"); + urdf::Box* box = (urdf::Box*)visual->geometry.get(); + btVector3 extents(box->dim.x, box->dim.y, box->dim.z); + 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 Geometry::SPHERE: + { + printf("processing a sphere\n"); + urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); + btScalar radius = sphere->radius; + btSphereShape* sphereShape = new btSphereShape(radius); + convexColShape = sphereShape; + convexColShape->setMargin(0.001); + break; + + break; + } + case Geometry::MESH: + { + if (visual->name.length()) + { + printf("visual->name=%s\n", visual->name.c_str()); + } + if (visual->geometry) + { + const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get(); + if (mesh->filename.length()) + { + const char* filename = mesh->filename.c_str(); + printf("mesh->filename=%s\n", filename); + char fullPath[1024]; + int fileType = 0; + sprintf(fullPath, "%s%s", pathPrefix, 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", pathPrefix, filename); + FILE* f = fopen(fullPath, "rb"); + if (f) + { + fclose(f); + + + + switch (fileType) + { + case FILE_OBJ: + { + glmesh = LoadMeshFromObj(fullPath,pathPrefix); + break; + } + + case FILE_STL: + { + glmesh = LoadMeshFromSTL(fullPath); + break; + } + case FILE_COLLADA: + { + + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + int upAxis = 2; + + LoadMeshFromCollada(fullPath, + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis); + + glmesh = new GLInstanceGraphicsShape; + int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_vertices->size(); i++) + { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + + } + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } + + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setIdentity(); +// upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices + additionalVertices); + + for (int v = 0; vm_vertices->push_back(verts[v]); + } + } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(fullPath); + + break; + } + default: + { + printf("Error: unsupported file type for Visual mesh: %s\n", fullPath); + btAssert(0); + } + } + + + if (glmesh && (glmesh->m_numvertices>0)) + { + } + else + { + printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath); + } + + } + else + { + printf("mesh geometry not found %s\n", fullPath); + } + + + } + } + + + break; + } + default: + { + printf("Error: unknown visual geometry type\n"); + } + } + + //if we have a convex, tesselate into localVertices/localIndices + if (convexColShape) + { + btShapeHull* hull = new btShapeHull(convexColShape); + hull->buildHull(0.0); + { + // int strideInBytes = 9*sizeof(float); + int numVertices = hull->numVertices(); + int numIndices = hull->numIndices(); + + + glmesh = new GLInstanceGraphicsShape; + int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + + for (int i = 0; i < numVertices; i++) + { + GLInstanceVertex vtx; + btVector3 pos = hull->getVertexPointer()[i]; + vtx.xyzw[0] = pos.x(); + vtx.xyzw[1] = pos.y(); + vtx.xyzw[2] = pos.z(); + vtx.xyzw[3] = 1.f; + pos.normalize(); + vtx.normal[0] = pos.x(); + vtx.normal[1] = pos.y(); + vtx.normal[2] = pos.z(); + vtx.uv[0] = 0.5f; + vtx.uv[1] = 0.5f; + glmesh->m_vertices->push_back(vtx); + } + + btAlignedObjectArray indices; + for (int i = 0; i < numIndices; i++) + { + glmesh->m_indices->push_back(hull->getIndexPointer()[i]); + } + + glmesh->m_numvertices = glmesh->m_vertices->size(); + glmesh->m_numIndices = glmesh->m_indices->size(); + } + delete convexColShape; + convexColShape = 0; + + } + + if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0) + { + + int baseIndex = verticesOut.size(); + + + + for (int i = 0; i < glmesh->m_indices->size(); i++) + { + indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex); + } + + for (int i = 0; i < glmesh->m_vertices->size(); i++) + { + GLInstanceVertex& v = glmesh->m_vertices->at(i); + btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]); + btVector3 vt = visualTransform*vert; + v.xyzw[0] = vt[0]; + v.xyzw[1] = vt[1]; + v.xyzw[2] = vt[2]; + btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]); + triNormal = visualTransform.getBasis()*triNormal; + v.normal[0] = triNormal[0]; + v.normal[1] = triNormal[1]; + v.normal[2] = triNormal[2]; + verticesOut.push_back(v); + } + } +} + + + + +btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix) +{ + btCollisionShape* shape = 0; + + switch (visual->geometry->type) + { + case Geometry::CYLINDER: + { + printf("processing a cylinder\n"); + urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); + + btAlignedObjectArray vertices; + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i=0;iradius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.); + vertices.push_back(vert); + vert[2] = -cyl->length/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 Geometry::BOX: + { + printf("processing a box\n"); + urdf::Box* box = (urdf::Box*)visual->geometry.get(); + btVector3 extents(box->dim.x,box->dim.y,box->dim.z); + 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 Geometry::SPHERE: + { + printf("processing a sphere\n"); + urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); + btScalar radius = sphere->radius; + btSphereShape* sphereShape = new btSphereShape(radius); + shape = sphereShape; + shape ->setMargin(0.001); + break; + + break; + } + case Geometry::MESH: + { + if (visual->name.length()) + { + printf("visual->name=%s\n",visual->name.c_str()); + } + if (visual->geometry) + { + const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get(); + if (mesh->filename.length()) + { + const char* filename = mesh->filename.c_str(); + printf("mesh->filename=%s\n",filename); + char fullPath[1024]; + int fileType = 0; + sprintf(fullPath,"%s%s",pathPrefix,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",pathPrefix,filename); + FILE* f = fopen(fullPath,"rb"); + if (f) + { + fclose(f); + GLInstanceGraphicsShape* glmesh = 0; + + + switch (fileType) + { + case FILE_OBJ: + { + glmesh = LoadMeshFromObj(fullPath,pathPrefix); + break; + } + case FILE_STL: + { + glmesh = LoadMeshFromSTL(fullPath); + break; + } + case FILE_COLLADA: + { + + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans;upAxisTrans.setIdentity(); + float unitMeterScaling=1; + int upAxis = 2; + LoadMeshFromCollada(fullPath, + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis ); + + glmesh = new GLInstanceGraphicsShape; + int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i=0;im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i=0;im_vertices->size();i++) + { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + + } + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices+additionalIndices); + for (int k=0;km_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex; + } + + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices+additionalVertices); + + for(int v=0;vm_vertices->push_back(verts[v]); + } + } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(fullPath); + + break; + } + default: + { + printf("Unsupported file type in Collision: %s\n",fullPath); + btAssert(0); + } + } + + + if (glmesh && (glmesh->m_numvertices>0)) + { + printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); + //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); + //convex->setUserIndex(shapeId); + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + for (int i=0;im_numvertices;i++) + { + convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2])); + } + //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex)); + btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); + //cylZShape->initializePolyhedralFeatures(); + //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); + //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); + cylZShape->setMargin(0.001); + shape = cylZShape; + } else + { + printf("issue extracting mesh from STL file %s\n", fullPath); + } + + } else + { + printf("mesh geometry not found %s\n",fullPath); + } + + + } + } + + + break; + } + default: + { + printf("Error: unknown visual geometry type\n"); + } + } + return shape; +} + + + + +int MyURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const +{ + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + int graphicsIndex = -1; + + 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); + + } + + 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* MyURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const +{ + + btCompoundShape* compoundShape = new btCompoundShape(); + compoundShape->setMargin(0.001); + + 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); + + } + } + + return compoundShape; +} diff --git a/examples/Importers/ImportURDFDemo/MyURDFImporter.h b/examples/Importers/ImportURDFDemo/MyURDFImporter.h new file mode 100644 index 000000000..84e86bf38 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/MyURDFImporter.h @@ -0,0 +1,41 @@ +#ifndef MY_URDF_IMPORTER_H +#define MY_URDF_IMPORTER_H + +#include "URDFImporterInterface.h" +#include //temp, replace by btAlignedObjectArray + +#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" + +class MyURDFImporter : public URDFImporterInterface +{ + + struct MyURDFInternalData* m_data; + + +public: + + + MyURDFImporter(my_shared_ptr robot,struct GUIHelperInterface* helper); + + virtual ~MyURDFImporter(); + + virtual int getRootLinkIndex() const; + + virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const; + + virtual std::string getLinkName(int linkIndex) const; + + virtual std::string getJointName(int linkIndex) const; + + virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const; + + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const; + + virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; + + virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; + +}; + + +#endif //MY_URDF_IMPORTER_H diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 510fe9915..c13c3a45a 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -1,4 +1,4 @@ -#include "URDF2Bullet.h" +#include "URDFImporterInterface.h" #include #include "LinearMath/btTransform.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" @@ -6,7 +6,9 @@ #include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" - +#include "URDFImporterInterface.h" +#include "MultiBodyCreationInterface.h" +#include static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter; static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter); static bool enableConstraints = true; @@ -30,7 +32,7 @@ static btVector3 selectColor2() return color; } -void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel) +void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel) { btAlignedObjectArray childIndices; u2b.getLinkChildIndices(linkIndex,childIndices); @@ -50,6 +52,7 @@ void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel) } } + struct URDF2BulletCachedData { URDF2BulletCachedData() @@ -104,7 +107,7 @@ struct URDF2BulletCachedData }; -void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex) +void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex) { btAlignedObjectArray childIndices; u2b.getLinkChildIndices(linkIndex,childIndices); @@ -121,7 +124,7 @@ void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& c } } -void ComputeParentIndices(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex) +void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex) { cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex; cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++; @@ -134,7 +137,7 @@ void ComputeParentIndices(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, } } -void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache) +void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache) { //compute the number of links, and compute parent indices array (and possibly other cached data?) cache.m_totalNumJoints1 = 0; @@ -156,7 +159,7 @@ void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache) } -void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) +void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) { printf("start converting/extracting data from URDF interface\n"); @@ -241,14 +244,14 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c if (!createMultiBody) { - btRigidBody* body = u2b.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape); + btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape); linkRigidBody = body; world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask); compoundShape->setUserIndex(graphicsIndex); - u2b.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex); + creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex); cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } else { @@ -258,7 +261,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c bool canSleep = false; bool isFixedBase = (mass==0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; - cache.m_bulletMultiBody = u2b.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); + cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } @@ -275,7 +278,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c bool disableParentCollision = true; switch (jointType) { - case URDF2Bullet::FixedJoint: + case URDFFixedJoint: { if (createMultiBody) { @@ -285,7 +288,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation(); cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision); - u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex); + creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); btMatrix3x3 rm(rot); btScalar y,p,r; @@ -302,7 +305,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c printf("y=%f,p=%f,r=%f\n", y,p,r); //we could also use btFixedConstraint but it has some issues - btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB); + btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB); dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); @@ -315,8 +318,8 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c } break; } - case URDF2Bullet::ContinuousJoint: - case URDF2Bullet::RevoluteJoint: + case URDFContinuousJoint: + case URDFRevoluteJoint: { if (createMultiBody) { @@ -326,7 +329,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); - u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex); + creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); } else { @@ -337,7 +340,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c { case 0: { - btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX); + btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX); dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); @@ -350,7 +353,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c } case 1: { - btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY); + btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY); dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); @@ -364,7 +367,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c case 2: default: { - btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ); + btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ); dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); @@ -379,7 +382,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c } break; } - case URDF2Bullet::PrismaticJoint: + case URDFPrismaticJoint: { if (createMultiBody) { @@ -389,11 +392,11 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c -offsetInB.getOrigin(), disableParentCollision); - u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex); + creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); } else { - btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); + btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); //todo(erwincoumans) for now, we only support principle axis along X, Y or Z int principleAxis = jointAxisInJointSpace.closestAxis(); switch (principleAxis) @@ -440,7 +443,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c { if (compoundShape->getNumChildShapes()>0) { - btMultiBodyLinkCollider* col= u2b.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody); + btMultiBodyLinkCollider* col= creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody); compoundShape->setUserIndex(graphicsIndex); @@ -462,7 +465,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c btVector3 color = selectColor2();//(0.0,0.0,0.5); - u2b.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); + creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); btScalar friction = 0.5f; @@ -489,18 +492,18 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c { int urdfChildLinkIndex = urdfChildIndices[i]; - ConvertURDF2BulletInternal(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix); + ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix); } } -void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) +void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) { URDF2BulletCachedData cache; InitURDF2BulletCache(u2b,cache); int urdfLinkIndex = u2b.getRootLinkIndex(); - ConvertURDF2BulletInternal(u2b, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix); + ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix); } diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index 2d2a17747..aeca98dd5 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -8,64 +8,21 @@ class btTransform; class btMultiBodyDynamicsWorld; class btTransform; -class URDF2Bullet -{ - -public: - enum { - RevoluteJoint=1, - PrismaticJoint, - ContinuousJoint, - FloatingJoint, - PlanarJoint, - FixedJoint, - }; - - ///return >=0 for the root link index, -1 if there is no root link - virtual int getRootLinkIndex() const = 0; - - ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range) - virtual std::string getLinkName(int linkIndex) const =0; - - virtual std::string getJointName(int linkIndex) const = 0; - - //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity. - virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0; - - ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed - virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray& childLinkIndices) const =0; - - virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0; - - virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0; - - ///create Bullet collision shapes from URDF 'Collision' objects, specified in inertial frame of the link. - virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0; - - virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const = 0; - - ///optionally create some graphical representation from a collision object, usually for visual debugging purposes. - virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) const = 0; - - virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) const =0; - - virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) const = 0; - - virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) const = 0; - - virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) const = 0; - - virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) const = 0; -}; +class URDFImporterInterface; +class MultiBodyCreationInterface; -void printTree(const URDF2Bullet& u2b, int linkIndex, int identationLevel=0); +void printTree(const URDFImporterInterface& u2b, int linkIndex, int identationLevel=0); -void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world,bool createMultiBody, const char* pathPrefix); - +void ConvertURDF2Bullet(const URDFImporterInterface& u2b, + MultiBodyCreationInterface& creationCallback, + const btTransform& rootTransformInWorldSpace, + btMultiBodyDynamicsWorld* world, + bool createMultiBody, + const char* pathPrefix); #endif //_URDF2BULLET_H diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h new file mode 100644 index 000000000..ea8bd46b5 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -0,0 +1,41 @@ +#ifndef URDF_IMPORTER_INTERFACE_H +#define URDF_IMPORTER_INTERFACE_H + +#include +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btTransform.h" +#include "URDFJointTypes.h" + +class URDFImporterInterface +{ + +public: + + + virtual ~URDFImporterInterface() {} + + + ///return >=0 for the root link index, -1 if there is no root link + virtual int getRootLinkIndex() const = 0; + + ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range) + virtual std::string getLinkName(int linkIndex) const =0; + + virtual std::string getJointName(int linkIndex) const = 0; + + //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity. + virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0; + + ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed + virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray& childLinkIndices) const =0; + + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0; + + ///quick hack: need to rethink the API/dependencies of this + virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0; + + virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0; +}; + +#endif //URDF_IMPORTER_INTERFACE_H + diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h new file mode 100644 index 000000000..3024536e4 --- /dev/null +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -0,0 +1,16 @@ +#ifndef URDF_JOINT_TYPES_H +#define URDF_JOINT_TYPES_H + + +enum +{ + URDFRevoluteJoint=1, + URDFPrismaticJoint, + URDFContinuousJoint, + URDFFloatingJoint, + URDFPlanarJoint, + URDFFixedJoint, +}; + + +#endif //URDF_JOINT_TYPES_H