diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index 63dceeb39..470a0edb7 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -2,6 +2,7 @@ #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" @@ -33,11 +34,13 @@ class MyURDF2Bullet : public URDF2Bullet my_shared_ptr m_robot; std::vector > m_links; GraphicsPhysicsBridge& m_gfxBridge; + mutable btMultiBody* m_bulletMultiBody; public: MyURDF2Bullet(my_shared_ptr robot,GraphicsPhysicsBridge& gfxBridge) :m_robot(robot), - m_gfxBridge(gfxBridge) + m_gfxBridge(gfxBridge), + m_bulletMultiBody(0) { m_robot->getLinks(m_links); @@ -53,7 +56,7 @@ public: if (m_links.size()) { int rootLinkIndex = m_robot->getRoot()->m_link_index; - btAssert(m_links[0]->m_link_index == rootLinkIndex); + // btAssert(m_links[0]->m_link_index == rootLinkIndex); return rootLinkIndex; } return -1; @@ -92,7 +95,7 @@ public: } } - virtual bool getParent2JointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const { jointLowerLimit = 0.f; jointUpperLimit = 0.f; @@ -146,7 +149,7 @@ public: } } - virtual int convertLinkVisuals(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const + virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { btAlignedObjectArray vertices; btAlignedObjectArray indices; @@ -175,7 +178,7 @@ public: } - virtual class btCompoundShape* convertLinkCollisions(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const + virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const { btCompoundShape* compoundShape = new btCompoundShape(); @@ -201,6 +204,34 @@ public: 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 createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const { @@ -212,25 +243,50 @@ public: m_gfxBridge.createCollisionObjectGraphicsObject(colObj,colorRgba); } + btMultiBody* getBulletMultiBody() + { + return m_bulletMultiBody; + } + }; -const char* fileNames[] = -{ - "r2d2.urdf", -}; +btAlignedObjectArray gFileNameArray; ImportUrdfSetup::ImportUrdfSetup() { static int count = 0; + gFileNameArray.clear(); + gFileNameArray.push_back("r2d2.urdf"); - sprintf(m_fileName,fileNames[count++]); - int sz = sizeof(fileNames)/sizeof(char*); - if (count>=sz) + //load additional urdf file names from file + + FILE* f = fopen("urdf_files.txt","r"); + if (f) + { + int result; + //warning: we don't avoid string buffer overflow in this basic example in fscanf + char fileName[1024]; + do + { + result = fscanf(f,"%s",fileName); + if (result==1) + { + gFileNameArray.push_back(fileName); + } + } while (result==1); + + fclose(f); + } + + int numFileNames = gFileNameArray.size(); + + if (count>=numFileNames) { count=0; } + sprintf(m_fileName,gFileNameArray[count++].c_str()); } ImportUrdfSetup::~ImportUrdfSetup() @@ -346,7 +402,8 @@ struct URDF2BulletMappings enum MyFileType { FILE_STL=1, - FILE_COLLADA=2 + FILE_COLLADA=2, + FILE_OBJ=3, }; @@ -431,6 +488,10 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons { fileType = FILE_STL; } + if (strstr(fullPath,".obj")) + { + fileType = FILE_OBJ; + } sprintf(fullPath, "%s%s", pathPrefix, filename); @@ -443,6 +504,12 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons switch (fileType) { + case FILE_OBJ: + { + glmesh = LoadMeshFromObj(fullPath,pathPrefix); + break; + } + case FILE_STL: { glmesh = LoadMeshFromSTL(fullPath); @@ -531,6 +598,8 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons } default: { + printf("Error: unsupported file type for Visual mesh: %s\n", fullPath); + btAssert(0); } } @@ -723,7 +792,10 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha { fileType = FILE_STL; } - + if (strstr(fullPath,".obj")) + { + fileType = FILE_OBJ; + } sprintf(fullPath,"%s%s",pathPrefix,filename); FILE* f = fopen(fullPath,"rb"); @@ -735,6 +807,11 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha switch (fileType) { + case FILE_OBJ: + { + glmesh = LoadMeshFromObj(fullPath,pathPrefix); + break; + } case FILE_STL: { glmesh = LoadMeshFromSTL(fullPath); @@ -821,6 +898,8 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha } default: { + printf("Unsupported file type in Collision: %s\n",fullPath); + btAssert(0); } } @@ -1422,7 +1501,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) //now print the tree using the new interface MyURDF2Bullet u2b(robot,gfxBridge); - u2b.printTree(0,0); + printTree(u2b, 0,0); btTransform identityTrans; identityTrans.setIdentity(); @@ -1434,7 +1513,8 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { URDF2BulletMappings mappings; - URDF2BulletCachedData cache; + + btMultiBody* mb = 0; if (!useUrdfInterfaceClass) { @@ -1442,25 +1522,21 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) mappings.m_totalNumJoints = numJoints; mappings.m_urdfLinkIndices2BulletLinkIndices.resize(numJoints+1,-2);//root and child links (=1+numJoints) URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix); + mb = mappings.m_bulletMultiBody; } else { - URDF2BulletConfig config; - config.m_createMultiBody = useFeatherstone; - //ConvertURDF2Bullet(URDF2Bullet& u2b, int linkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,URDF2BulletConfig& config, const char* pathPrefix) - + //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,useFeatherstone,pathPrefix); + mb = u2b.getBulletMultiBody(); - - InitURDF2BulletCache(u2b,cache); - ConvertURDF2Bullet(u2b,cache,rootLinkIndex,identityTrans,m_dynamicsWorld,config,pathPrefix); } if (useFeatherstone) { - btMultiBody* mb = useUrdfInterfaceClass? cache.m_bulletMultiBody : mappings.m_bulletMultiBody; - mb->setHasSelfCollision(false); + mb->setHasSelfCollision(false); mb->finalizeMultiDof(); m_dynamicsWorld->addMultiBody(mb); } @@ -1482,7 +1558,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) gfxBridge.createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); - groundOrigin[upAxis]=-1.5; + groundOrigin[upAxis]=-2;//.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); //m_dynamicsWorld->removeRigidBody(body); diff --git a/Demos3/ImportURDFDemo/URDF2Bullet.cpp b/Demos3/ImportURDFDemo/URDF2Bullet.cpp index 9f07be35b..656996187 100644 --- a/Demos3/ImportURDFDemo/URDF2Bullet.cpp +++ b/Demos3/ImportURDFDemo/URDF2Bullet.cpp @@ -30,10 +30,10 @@ static btVector3 selectColor2() return color; } -void URDF2Bullet::printTree(int linkIndex, int indentationLevel) +void printTree(const URDF2Bullet& u2b, int linkIndex, int indentationLevel) { btAlignedObjectArray childIndices; - getLinkChildIndices(linkIndex,childIndices); + u2b.getLinkChildIndices(linkIndex,childIndices); int numChildren = childIndices.size(); @@ -42,14 +42,68 @@ void URDF2Bullet::printTree(int linkIndex, int indentationLevel) for (int i=0;i m_urdfLinkParentIndices; + btAlignedObjectArray m_urdfLinkIndices2BulletLinkIndices; + btAlignedObjectArray m_urdfLink2rigidBodies; + btAlignedObjectArray m_urdfLinkLocalInertialFrames; + + int m_currentMultiBodyLinkIndex; + + class btMultiBody* m_bulletMultiBody; + + //this will be initialized in the constructor + int m_totalNumJoints1; + int getParentUrdfIndex(int linkIndex) const + { + return m_urdfLinkParentIndices[linkIndex]; + } + int getMbIndexFromUrdfIndex(int urdfIndex) const + { + if (urdfIndex==-2) + return -2; + return m_urdfLinkIndices2BulletLinkIndices[urdfIndex]; + } + + + void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame) + { + m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame; + } + + class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex) + { + return m_urdfLink2rigidBodies[urdfLinkIndex]; + } + + void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame) + { + btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0); + + m_urdfLink2rigidBodies[urdfLinkIndex] = body; + m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame; + } + +}; + void ComputeTotalNumberOfJoints(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex) { btAlignedObjectArray childIndices; @@ -102,7 +156,7 @@ void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache) } -void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,const URDF2BulletConfig& mappings, const char* pathPrefix) +void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) { printf("start converting/extracting data from URDF interface\n"); @@ -156,23 +210,24 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in btScalar jointUpperLimit; - bool hasParentJoint = u2b.getParent2JointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit); + bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit); linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; - int graphicsIndex = u2b.convertLinkVisuals(urdfLinkIndex,pathPrefix,localInertialFrame); + int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame); - btCompoundShape* compoundShape = u2b.convertLinkCollisions(urdfLinkIndex,pathPrefix,localInertialFrame); + btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame); if (compoundShape) { btVector3 color = selectColor2(); - /* if (visual->material.get()) + /* + if (visual->material.get()) { - color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); + color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); } */ //btVector3 localInertiaDiagonal(0, 0, 0); @@ -182,18 +237,11 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in //} btRigidBody* linkRigidBody = 0; - - //btTransform visualFrameInWorldSpace = linkTransformInWorldSpace*visual_frame; btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame; - // URDF_LinkInformation* linkInfo = new URDF_LinkInformation; - if (!mappings.m_createMultiBody) + if (!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); + btRigidBody* body = u2b.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape); linkRigidBody = body; world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask); @@ -201,13 +249,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in compoundShape->setUserIndex(graphicsIndex); u2b.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex); - -// gfxBridge.createRigidBodyGraphicsObject(body, color); - - // linkInfo->m_bulletRigidBody = body; - cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); - } else { if (cache.m_bulletMultiBody==0) @@ -216,74 +258,39 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in bool canSleep = false; bool isFixedBase = (mass==0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; - cache.m_bulletMultiBody = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); + cache.m_bulletMultiBody = u2b.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } } - - // 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 (hasParentJoint)//(*link).parent_joint && pp) - { + if (hasParentJoint) { -// 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 = parentLocalInertialFrame.inverse()*parent2joint; - - offsetInB.setIdentity(); - //offsetInB = visual_frame.inverse(); offsetInB = localInertialFrame.inverse(); - bool disableParentCollision = true; switch (jointType) { case URDF2Bullet::FixedJoint: { - if (mappings.m_createMultiBody) + if (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); cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, rot*offsetInB.getRotation(), offsetInA.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"); @@ -291,12 +298,11 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in 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(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); - // btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z); + //we could also use btFixedConstraint but it has some issues + btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB); + dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); @@ -305,34 +311,20 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in if (enableConstraints) world1->addConstraint(dof6,true); - - // btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB); - // world->addConstraint(fixed,true); } break; } case URDF2Bullet::ContinuousJoint: case URDF2Bullet::RevoluteJoint: { - if (mappings.m_createMultiBody) + if (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); - */ - + cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, - //parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(), offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), - -offsetInB.getOrigin(), disableParentCollision); - //linkInfo->m_localVisualFrame.setIdentity(); } else { @@ -343,7 +335,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in { case 0: { - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX); + btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX); dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); @@ -356,7 +348,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in } case 1: { - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY); + btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY); dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); @@ -370,7 +362,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in case 2: default: { - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ); + btGeneric6DofSpring2Constraint* dof6 = u2b.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ); dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); @@ -387,7 +379,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in } case URDF2Bullet::PrismaticJoint: { - if (mappings.m_createMultiBody) + if (createMultiBody) { cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, @@ -399,8 +391,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in } else { - - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); + btGeneric6DofSpring2Constraint* dof6 = u2b.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) @@ -437,21 +428,18 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in default: { printf("Error: unsupported joint type in URDF (%d)\n", jointType); - btAssert(0); + btAssert(0); } } } - if (mappings.m_createMultiBody) + if (createMultiBody) { if (compoundShape->getNumChildShapes()>0) { - btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(cache.m_bulletMultiBody, mbLinkIndex); //or mbLinkIndex-1??? double-check - - //btCompoundShape* comp = new btCompoundShape(); - //comp->addChildShape(linkInfo->m_localVisualFrame,shape); - + btMultiBodyLinkCollider* col= u2b.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody); + compoundShape->setUserIndex(graphicsIndex); col->setCollisionShape(compoundShape); @@ -462,8 +450,6 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in //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; @@ -501,7 +487,19 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in { int urdfChildLinkIndex = urdfChildIndices[i]; - ConvertURDF2Bullet(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,mappings,pathPrefix); + ConvertURDF2BulletInternal(u2b,cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix); } -} \ No newline at end of file +} + +void ConvertURDF2Bullet(const URDF2Bullet& u2b, 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); + +} + + diff --git a/Demos3/ImportURDFDemo/URDF2Bullet.h b/Demos3/ImportURDFDemo/URDF2Bullet.h index 43d3fc292..9f4f83472 100644 --- a/Demos3/ImportURDFDemo/URDF2Bullet.h +++ b/Demos3/ImportURDFDemo/URDF2Bullet.h @@ -5,115 +5,14 @@ #include class btVector3; class btTransform; - -struct URDF2BulletConfig -{ - URDF2BulletConfig() - :m_createMultiBody(true) - { - - } - - //true to create a btMultiBody, false to use btRigidBody - bool m_createMultiBody; -}; - -struct URDF2BulletCachedData -{ - URDF2BulletCachedData() - :m_totalNumJoints1(0), - m_currentMultiBodyLinkIndex(-1), - m_bulletMultiBody(0) - { - - } - //these arrays will be initialized in the 'InitURDF2BulletCache' - - btAlignedObjectArray m_urdfLinkParentIndices; - btAlignedObjectArray m_urdfLinkIndices2BulletLinkIndices; - btAlignedObjectArray m_urdfLink2rigidBodies; - btAlignedObjectArray m_urdfLinkLocalInertialFrames; - - int m_currentMultiBodyLinkIndex; - - class btMultiBody* m_bulletMultiBody; - - //this will be initialized in the constructor - int m_totalNumJoints1; - int getParentUrdfIndex(int linkIndex) const - { - return m_urdfLinkParentIndices[linkIndex]; - } - int getMbIndexFromUrdfIndex(int urdfIndex) const - { - if (urdfIndex==-2) - return -2; - return m_urdfLinkIndices2BulletLinkIndices[urdfIndex]; - } - - - void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame) - { - m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame; - } - - class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex) - { - return m_urdfLink2rigidBodies[urdfLinkIndex]; - } - - void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame) - { - btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0); - - m_urdfLink2rigidBodies[urdfLinkIndex] = body; - m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame; - - /* - struct URDF_LinkInformation - { - const Link* m_thisLink; - int m_linkIndex; - //int m_parentIndex; - - btTransform m_localInertialFrame; - //btTransform m_localVisualFrame; - btTransform m_bodyWorldTransform; - btVector3 m_localInertiaDiagonal; - btScalar m_mass; - - btCollisionShape* m_collisionShape; - btRigidBody* m_bulletRigidBody; - - URDF_LinkInformation() - :m_thisLink(0), - m_linkIndex(-2), - //m_parentIndex(-2), - m_collisionShape(0), - m_bulletRigidBody(0) - { - - } - virtual ~URDF_LinkInformation() - { - printf("~\n"); - } - }; - */ - - } - - -}; - class btMultiBodyDynamicsWorld; class btTransform; class URDF2Bullet { - public: + enum { RevoluteJoint=1, PrismaticJoint, @@ -122,8 +21,7 @@ public: PlanarJoint, FixedJoint, }; - void printTree(int linkIndex, int indentationLevel=0); - + ///return >=0 for the root link index, -1 if there is no root link virtual int getRootLinkIndex() const = 0; @@ -136,23 +34,35 @@ public: ///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 getParent2JointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0; + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0; - virtual int convertLinkVisuals(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0; + virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertialFrame) const=0; - virtual class btCompoundShape* convertLinkCollisions(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) 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, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) 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; + }; -void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache); +void printTree(const URDF2Bullet& u2b, int linkIndex, int identationLevel=0); + + + +void ConvertURDF2Bullet(const URDF2Bullet& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world,bool createMultiBody, const char* pathPrefix); + -void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, int linkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,const URDF2BulletConfig& mappings, const char* pathPrefix); - #endif //_URDF2BULLET_H diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h index e55ee235a..20c643605 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -58,7 +58,7 @@ class btRigidBody; enum RotateOrder { - RO_XYZ, + RO_XYZ=0, RO_XZY, RO_YXZ, RO_YZX,