void GetAllIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int parentIndex, btAlignedObjectArray<childParentIndex>& allIndices) { childParentIndex cp; cp.m_index = urdfLinkIndex; int mbIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex); cp.m_mbIndex = mbIndex; cp.m_parentIndex = parentIndex; int parentMbIndex = parentIndex>=0? cache.getMbIndexFromUrdfIndex(parentIndex) : -1; cp.m_parentMBIndex = parentMbIndex; allIndices.push_back(cp); btAlignedObjectArray<int> urdfChildIndices; u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices); int numChildren = urdfChildIndices.size(); for (int i = 0; i < numChildren; i++) { int urdfChildLinkIndex = urdfChildIndices[i]; GetAllIndices(u2b, cache, urdfChildLinkIndex, urdfLinkIndex, allIndices); } }
void ConvertURDF2BulletInternal( const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, bool createMultiBody, const char* pathPrefix, int flags = 0) { //b3Printf("start converting/extracting data from URDF interface\n"); btTransform linkTransformInWorldSpace; linkTransformInWorldSpace.setIdentity(); int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex); int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex); int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex); btRigidBody* parentRigidBody = 0; //b3Printf("mb link index = %d\n",mbLinkIndex); btTransform parentLocalInertialFrame; parentLocalInertialFrame.setIdentity(); btScalar parentMass(1); btVector3 parentLocalInertiaDiagonal(1,1,1); if (urdfParentIndex==-2) { //b3Printf("root link has no parent\n"); } else { //b3Printf("urdf parent index = %d\n",urdfParentIndex); //b3Printf("mb parent index = %d\n",mbParentIndex); parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex); u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame); } btScalar mass = 0; btTransform localInertialFrame; localInertialFrame.setIdentity(); btVector3 localInertiaDiagonal(0,0,0); u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); btTransform parent2joint; parent2joint.setIdentity(); int jointType; btVector3 jointAxisInJointSpace; btScalar jointLowerLimit; btScalar jointUpperLimit; btScalar jointDamping; btScalar jointFriction; bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); if (flags & CUF_USE_SDF) { parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace; } else { linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; } btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame); int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame); if (compoundShape) { btVector3 color = selectColor2(); /* if (visual->material.get()) { color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); } */ if (mass) { if (!(flags & CUF_USE_URDF_INERTIA)) { compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); //temporary inertia scaling until we load inertia from URDF if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING) { localInertiaDiagonal*=contactInfo.m_inertiaScaling; } } btRigidBody* linkRigidBody = 0; btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame; if (!createMultiBody) { btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape); linkRigidBody = body; world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask); compoundShape->setUserIndex(graphicsIndex); creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex); cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body); } else { if (cache.m_bulletMultiBody==0) { bool canSleep = false; bool isFixedBase = (mass==0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep); cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } } //create a joint if necessary if (hasParentJoint) { btTransform offsetInA,offsetInB; offsetInA = parentLocalInertialFrame.inverse()*parent2joint; offsetInB = localInertialFrame.inverse(); btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation(); bool disableParentCollision = true; switch (jointType) { case URDFFixedJoint: { if (createMultiBody) { //todo: adjust the center of mass transform and pivot axis properly cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, parentRotToThis, offsetInA.getOrigin(),-offsetInB.getOrigin()); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); } else { //b3Printf("Fixed joint\n"); btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA); if (enableConstraints) world1->addConstraint(dof6,true); } break; } case URDFContinuousJoint: case URDFRevoluteJoint: { if (createMultiBody) { cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction; creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); if (jointLowerLimit <= jointUpperLimit) { //std::string name = u2b.getLinkName(urdfLinkIndex); //printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit); btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); world1->addMultiBodyConstraint(con); } } else { btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); if (enableConstraints) world1->addConstraint(dof6,true); //b3Printf("Revolute/Continuous joint\n"); } break; } case URDFPrismaticJoint: { if (createMultiBody) { cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); if (jointLowerLimit <= jointUpperLimit) { //std::string name = u2b.getLinkName(urdfLinkIndex); //printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit); btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); world1->addMultiBodyConstraint(con); } //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); } else { btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); if (enableConstraints) world1->addConstraint(dof6,true); //b3Printf("Prismatic\n"); } break; } default: { //b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType); btAssert(0); } } } if (createMultiBody) { //if (compoundShape->getNumChildShapes()>0) { btMultiBodyLinkCollider* col= creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody); 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 col->setWorldTransform(tr); //base and fixed? -> static, otherwise flag as dynamic bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : 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); btVector4 color = selectColor2();//(0.0,0.0,0.5); u2b.getLinkColor(urdfLinkIndex,color); creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId()); URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0) { col->setFriction(contactInfo.m_lateralFriction); } if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0) { col->setRollingFriction(contactInfo.m_rollingFriction); } if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION)!=0) { col->setSpinningFriction(contactInfo.m_spinningFriction); } if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0) { col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping); } if (mbLinkIndex>=0) //???? double-check +/- 1 { cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col; } else { cache.m_bulletMultiBody->setBaseCollider(col); } } } else { //u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape); } } btAlignedObjectArray<int> urdfChildIndices; u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices); int numChildren = urdfChildIndices.size(); for (int i=0;i<numChildren;i++) { int urdfChildLinkIndex = urdfChildIndices[i]; ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags); } }
btTransform ConvertURDF2BulletInternal( const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut, bool recursive) { B3_PROFILE("ConvertURDF2BulletInternal2"); //b3Printf("start converting/extracting data from URDF interface\n"); btTransform linkTransformInWorldSpace; linkTransformInWorldSpace.setIdentity(); int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex); int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex); int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex); btRigidBody* parentRigidBody = 0; //b3Printf("mb link index = %d\n",mbLinkIndex); btTransform parentLocalInertialFrame; parentLocalInertialFrame.setIdentity(); btScalar parentMass(1); btVector3 parentLocalInertiaDiagonal(1, 1, 1); if (urdfParentIndex == -2) { //b3Printf("root link has no parent\n"); } else { //b3Printf("urdf parent index = %d\n",urdfParentIndex); //b3Printf("mb parent index = %d\n",mbParentIndex); parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex); u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags); } btScalar mass = 0; btTransform localInertialFrame; localInertialFrame.setIdentity(); btVector3 localInertiaDiagonal(0, 0, 0); u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags); btTransform parent2joint; parent2joint.setIdentity(); int jointType; btVector3 jointAxisInJointSpace; btScalar jointLowerLimit; btScalar jointUpperLimit; btScalar jointDamping; btScalar jointFriction; btScalar jointMaxForce; btScalar jointMaxVelocity; bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity); std::string linkName = u2b.getLinkName(urdfLinkIndex); if (flags & CUF_USE_SDF) { parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace; } else { if (flags & CUF_USE_MJCF) { linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace; } else { linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint; } } btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, localInertialFrame); btCollisionShape* compoundShape = tmpShape; if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0) == btTransform::getIdentity()) { compoundShape = tmpShape->getChildShape(0); } int graphicsIndex; { B3_PROFILE("convertLinkVisualShapes"); if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1)) { graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1]; UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1]; u2b.setLinkColor2(urdfLinkIndex, matColor); } else { graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, localInertialFrame); if (cachedLinkGraphicsShapesOut) { cachedLinkGraphicsShapesOut->m_cachedUrdfLinkVisualShapeIndices.push_back(graphicsIndex); UrdfMaterialColor matColor; u2b.getLinkColor2(urdfLinkIndex, matColor); cachedLinkGraphicsShapesOut->m_cachedUrdfLinkColors.push_back(matColor); } } } if (compoundShape) { UrdfMaterialColor matColor; btVector4 color2 = selectColor2(); btVector3 specular(0.5, 0.5, 0.5); if (u2b.getLinkColor2(urdfLinkIndex, matColor)) { color2 = matColor.m_rgbaColor; specular = matColor.m_specularColor; } /* if (visual->material.get()) { color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); } */ if (mass) { if (!(flags & CUF_USE_URDF_INERTIA)) { compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); btAssert(localInertiaDiagonal[0] < 1e10); btAssert(localInertiaDiagonal[1] < 1e10); btAssert(localInertiaDiagonal[2] < 1e10); } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex, contactInfo); //temporary inertia scaling until we load inertia from URDF if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING) { localInertiaDiagonal *= contactInfo.m_inertiaScaling; } } btRigidBody* linkRigidBody = 0; btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame; bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0; if (!createMultiBody) { btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape); if (!canSleep) { body->forceActivationState(DISABLE_DEACTIVATION); } linkRigidBody = body; world1->addRigidBody(body); compoundShape->setUserIndex(graphicsIndex); URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex, contactInfo); processContactParameters(contactInfo, body); creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex); cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); //untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body); } else { if (cache.m_bulletMultiBody == 0) { bool isFixedBase = (mass == 0); //todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep); if (flags & CUF_GLOBAL_VELOCITIES_MB) { cache.m_bulletMultiBody->useGlobalVelocities(true); } if (flags & CUF_USE_MJCF) { cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace); } cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } } //create a joint if necessary if (hasParentJoint) { btTransform offsetInA, offsetInB; offsetInA = parentLocalInertialFrame.inverse() * parent2joint; offsetInB = localInertialFrame.inverse(); btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation(); bool disableParentCollision = true; if (createMultiBody && cache.m_bulletMultiBody) { cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce = jointMaxForce; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity; } switch (jointType) { case URDFSphericalJoint: { if (createMultiBody) { creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); cache.m_bulletMultiBody->setupSpherical(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); } else { btAssert(0); } break; } case URDFPlanarJoint: { if (createMultiBody) { #if 0 void setupPlanar(int i, // 0 to num_links-1 btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame bool disableParentCollision = false); #endif creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); cache.m_bulletMultiBody->setupPlanar(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), disableParentCollision); } else { #if 0 //b3Printf("Fixed joint\n"); btGeneric6DofSpring2Constraint* dof6 = 0; //backward compatibility if (flags & CUF_RESERVED) { dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB); } else { dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA); } if (enableConstraints) world1->addConstraint(dof6, true); #endif } break; } case URDFFloatingJoint: case URDFFixedJoint: { if ((jointType == URDFFloatingJoint) || (jointType == URDFPlanarJoint)) { printf("Warning: joint unsupported, creating a fixed joint instead."); } creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); if (createMultiBody) { //todo: adjust the center of mass transform and pivot axis properly cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin()); } else { //b3Printf("Fixed joint\n"); btGeneric6DofSpring2Constraint* dof6 = 0; //backward compatibility if (flags & CUF_RESERVED) { dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB); } else { dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA); } if (enableConstraints) world1->addConstraint(dof6, true); } break; } case URDFContinuousJoint: case URDFRevoluteJoint: { creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); if (createMultiBody) { cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) { //std::string name = u2b.getLinkName(urdfLinkIndex); //printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit); btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); world1->addMultiBodyConstraint(con); } } else { btGeneric6DofSpring2Constraint* dof6 = 0; if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) { //backwards compatibility if (flags & CUF_RESERVED) { dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit); } else { dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit); } } else { //disable joint limits if (flags & CUF_RESERVED) { dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, 1, -1); } else { dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, 1, -1); } } if (enableConstraints) world1->addConstraint(dof6, true); //b3Printf("Revolute/Continuous joint\n"); } break; } case URDFPrismaticJoint: { creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); if (createMultiBody) { cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); if (jointLowerLimit <= jointUpperLimit) { //std::string name = u2b.getLinkName(urdfLinkIndex); //printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit); btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); world1->addMultiBodyConstraint(con); } //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); } else { btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit); if (enableConstraints) world1->addConstraint(dof6, true); //b3Printf("Prismatic\n"); } break; } default: { //b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType); btAssert(0); } } } if (createMultiBody) { //if (compoundShape->getNumChildShapes()>0) { btMultiBodyLinkCollider* col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody); compoundShape->setUserIndex(graphicsIndex); col->setCollisionShape(compoundShape); if (compoundShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)compoundShape; if (trimeshShape->getTriangleInfoMap()) { col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } 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 col->setWorldTransform(tr); //base and fixed? -> static, otherwise flag as dynamic bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody->hasFixedBase()) ? false : true; int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); int colGroup = 0, colMask = 0; int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, colGroup, colMask); if (collisionFlags & URDF_HAS_COLLISION_GROUP) { collisionFilterGroup = colGroup; } if (collisionFlags & URDF_HAS_COLLISION_MASK) { collisionFilterMask = colMask; } world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); btVector4 color2 = selectColor2(); //(0.0,0.0,0.5); btVector3 specularColor(1, 1, 1); UrdfMaterialColor matCol; if (u2b.getLinkColor2(urdfLinkIndex, matCol)) { color2 = matCol.m_rgbaColor; specularColor = matCol.m_specularColor; } { B3_PROFILE("createCollisionObjectGraphicsInstance2"); creation.createCollisionObjectGraphicsInstance2(urdfLinkIndex, col, color2, specularColor); } { B3_PROFILE("convertLinkVisualShapes2"); u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId()); } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex, contactInfo); processContactParameters(contactInfo, col); if (mbLinkIndex >= 0) //???? double-check +/- 1 { cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider = col; if (flags & CUF_USE_SELF_COLLISION_INCLUDE_PARENT) { cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags &= ~BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; } if (flags & CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) { cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION; } } else { // if (canSleep) { if (cache.m_bulletMultiBody->getBaseMass() == 0) //&& cache.m_bulletMultiBody->getNumDofs()==0) { //col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT); } } cache.m_bulletMultiBody->setBaseCollider(col); } } } else { int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex); //u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId()); u2b.convertLinkVisualShapes2(-1, urdfLinkIndex, pathPrefix, localInertialFrame, linkRigidBody, u2b.getBodyUniqueId()); } } btAlignedObjectArray<int> urdfChildIndices; u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices); int numChildren = urdfChildIndices.size(); if (recursive) { for (int i = 0; i < numChildren; i++) { int urdfChildLinkIndex = urdfChildIndices[i]; ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive); } } return linkTransformInWorldSpace; }
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"); btTransform linkTransformInWorldSpace; linkTransformInWorldSpace.setIdentity(); int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex); int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex); int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex); btRigidBody* parentRigidBody = 0; std::string name = u2b.getLinkName(urdfLinkIndex); printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex); printf("mb link index = %d\n",mbLinkIndex); btTransform parentLocalInertialFrame; parentLocalInertialFrame.setIdentity(); btScalar parentMass(1); btVector3 parentLocalInertiaDiagonal(1,1,1); if (urdfParentIndex==-2) { printf("root link has no parent\n"); } else { printf("urdf parent index = %d\n",urdfParentIndex); printf("mb parent index = %d\n",mbParentIndex); parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex); u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame); } btScalar mass = 0; btTransform localInertialFrame; localInertialFrame.setIdentity(); btVector3 localInertiaDiagonal(0,0,0); u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); btTransform parent2joint; parent2joint.setIdentity(); int jointType; btVector3 jointAxisInJointSpace; btScalar jointLowerLimit; btScalar jointUpperLimit; bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit); linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame); btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame); if (compoundShape) { btVector3 color = selectColor2(); /* 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); //} btRigidBody* linkRigidBody = 0; btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame; if (!createMultiBody) { btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape); linkRigidBody = body; world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask); compoundShape->setUserIndex(graphicsIndex); creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex); cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } else { if (cache.m_bulletMultiBody==0) { bool multiDof = true; bool canSleep = false; bool isFixedBase = (mass==0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } } //create a joint if necessary if (hasParentJoint) { btTransform offsetInA,offsetInB; offsetInA = parentLocalInertialFrame.inverse()*parent2joint; offsetInB = localInertialFrame.inverse(); bool disableParentCollision = true; switch (jointType) { case URDFFixedJoint: { if (createMultiBody) { //todo: adjust the center of mass transform and pivot axis properly printf("Fixed joint (btMultiBody)\n"); btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation(); cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); } else { printf("Fixed joint\n"); btMatrix3x3 rm(offsetInA.getBasis()); btScalar y,p,r; rm.getEulerZYX(y,p,r); printf("y=%f,p=%f,r=%f\n", y,p,r); //we could also use btFixedConstraint but it has some issues btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB); 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); } break; } case URDFContinuousJoint: case URDFRevoluteJoint: { if (createMultiBody) { cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); } else { //only handle principle axis at the moment, //@todo(erwincoumans) orient the constraint for non-principal axis int principleAxis = jointAxisInJointSpace.closestAxis(); switch (principleAxis) { case 0: { btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, 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 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, 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 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, 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 URDFPrismaticJoint: { if (createMultiBody) { cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit); //world1->addMultiBodyConstraint(con); printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); } else { 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) { case 0: { dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0)); dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0)); break; } case 1: { dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0)); dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0)); break; } case 2: default: { dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit)); dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit)); } }; 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", jointType); btAssert(0); } } } if (createMultiBody) { if (compoundShape->getNumChildShapes()>0) { btMultiBodyLinkCollider* col= creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody); 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 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 = selectColor2();//(0.0,0.0,0.5); creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); btScalar friction = 0.5f; col->setFriction(friction); if (mbLinkIndex>=0) //???? double-check +/- 1 { cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col; } else { cache.m_bulletMultiBody->setBaseCollider(col); } } } } btAlignedObjectArray<int> urdfChildIndices; u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices); int numChildren = urdfChildIndices.size(); for (int i=0;i<numChildren;i++) { int urdfChildLinkIndex = urdfChildIndices[i]; ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix); } }