From d8b08eaf2a0c8dab882c6f71a50704681cce9f69 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 13 Feb 2015 16:18:16 -0800 Subject: [PATCH] fix in URDF loading: apply visual transform locally, not after compensating for inertia transform. add option in COLLADA loading to specify client up axis (so that if client=Z and COLLADA =Z, upAxisTransform is identity, while if client=Y and COLLADA=Y, upAxisTransform is identity etc) --- .../ImportColladaDemo/ImportColladaSetup.cpp | 5 +- .../ImportColladaDemo/LoadMeshFromCollada.cpp | 70 ++++++++++++++----- .../ImportColladaDemo/LoadMeshFromCollada.h | 3 +- Demos3/ImportURDFDemo/ImportURDFSetup.cpp | 51 ++++++++------ 4 files changed, 86 insertions(+), 43 deletions(-) diff --git a/Demos3/ImportColladaDemo/ImportColladaSetup.cpp b/Demos3/ImportColladaDemo/ImportColladaSetup.cpp index 342883860..da2f1a9d2 100644 --- a/Demos3/ImportColladaDemo/ImportColladaSetup.cpp +++ b/Demos3/ImportColladaDemo/ImportColladaSetup.cpp @@ -46,7 +46,8 @@ static int ColladaGraphicsInstanceSortfnc(const ColladaGraphicsInstance& a,const void ImportColladaSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { - gfxBridge.setUpAxis(1); + int upAxis=1; + gfxBridge.setUpAxis(upAxis); this->createEmptyDynamicsWorld(); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); @@ -112,7 +113,7 @@ void ImportColladaSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { fileIndex = 0; } - LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling); + LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling, upAxis); #endif// COMPARE_WITH_ASSIMP diff --git a/Demos3/ImportColladaDemo/LoadMeshFromCollada.cpp b/Demos3/ImportColladaDemo/LoadMeshFromCollada.cpp index 864e88ee1..f5b345ec4 100644 --- a/Demos3/ImportColladaDemo/LoadMeshFromCollada.cpp +++ b/Demos3/ImportColladaDemo/LoadMeshFromCollada.cpp @@ -485,8 +485,10 @@ void readVisualSceneInstanceGeometries(TiXmlDocument& doc, btHashMapFirstChildElement("asset")->FirstChildElement("unit"); if (unitMeter) { @@ -498,25 +500,59 @@ void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr, TiXmlElement* upAxisElem = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("up_axis"); if (upAxisElem) { - std::string upAxisTxt = upAxisElem->GetText(); - if (upAxisTxt == "X_UP") + switch (clientUpAxis) { - btQuaternion y2x(btVector3(0,0,1),SIMD_HALF_PI); - tr.setRotation(y2x); - } - if (upAxisTxt == "Y_UP") - { - //assume Y_UP for now, to be compatible with assimp? - } - if (upAxisTxt == "Z_UP") - { - btQuaternion y2z(btVector3(1,0,0),-SIMD_HALF_PI); - tr.setRotation(y2z); - } + + case 1: + { + std::string upAxisTxt = upAxisElem->GetText(); + if (upAxisTxt == "X_UP") + { + btQuaternion x2y(btVector3(0,0,1),SIMD_HALF_PI); + tr.setRotation(x2y); + } + if (upAxisTxt == "Y_UP") + { + //assume Y_UP for now, to be compatible with assimp? + //client and COLLADA are both Z_UP so no transform needed (identity) + } + if (upAxisTxt == "Z_UP") + { + btQuaternion z2y(btVector3(1,0,0),-SIMD_HALF_PI); + tr.setRotation(z2y); + } + break; + } + case 2: + { + std::string upAxisTxt = upAxisElem->GetText(); + if (upAxisTxt == "X_UP") + { + btQuaternion x2z(btVector3(0,1,0),-SIMD_HALF_PI); + tr.setRotation(x2z); + } + if (upAxisTxt == "Y_UP") + { + btQuaternion y2z(btVector3(1,0,0),SIMD_HALF_PI); + tr.setRotation(y2z); + } + if (upAxisTxt == "Z_UP") + { + //client and COLLADA are both Z_UP so no transform needed (identity) + } + break; + } + case 0: + default: + { + //we don't support X or other up axis + btAssert(0); + } + }; } } -void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray& visualShapes, btAlignedObjectArray& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling) +void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray& visualShapes, btAlignedObjectArray& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling,int clientUpAxis) { GLInstanceGraphicsShape* instance = 0; @@ -543,7 +579,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray& visualShapes, btAlignedObjectArray& visualShapeInstances, btTransform& upAxisTrans, - float& unitMeterScaling); + float& unitMeterScaling, + int clientUpAxis); //#define COMPARE_WITH_ASSIMP #ifdef COMPARE_WITH_ASSIMP diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index ecd4d9125..de9754a1c 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -13,10 +13,21 @@ static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphasePr static bool enableConstraints = true;//false; +const char* fileNames[] = +{ + "r2d2.urdf", +}; ImportUrdfSetup::ImportUrdfSetup() { - sprintf(m_fileName,"r2d2.urdf"); + static int count = 0; + + sprintf(m_fileName,fileNames[count++]); + int sz = sizeof(fileNames)/sizeof(char*); + if (count>=sz) + { + count=0; + } } ImportUrdfSetup::~ImportUrdfSetup() @@ -261,12 +272,14 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons btAlignedObjectArray visualShapeInstances; btTransform upAxisTrans; upAxisTrans.setIdentity(); float unitMeterScaling = 1; + int upAxis = 2; LoadMeshFromCollada(fullPath, visualShapes, visualShapeInstances, upAxisTrans, - unitMeterScaling); + unitMeterScaling, + upAxis); glmesh = new GLInstanceGraphicsShape; int index = 0; @@ -307,7 +320,8 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons //compensate upAxisTrans and unitMeterScaling here btMatrix4x4 upAxisMat; - upAxisMat.setPureRotation(upAxisTrans.getRotation()); + upAxisMat.setIdentity(); +// upAxisMat.setPureRotation(upAxisTrans.getRotation()); btMatrix4x4 unitMeterScalingMat; unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; @@ -550,12 +564,13 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha btAlignedObjectArray visualShapeInstances; btTransform upAxisTrans;upAxisTrans.setIdentity(); float unitMeterScaling=1; - + int upAxis = 2; LoadMeshFromCollada(fullPath, visualShapes, visualShapeInstances, upAxisTrans, - unitMeterScaling); + unitMeterScaling, + upAxis ); glmesh = new GLInstanceGraphicsShape; int index = 0; @@ -599,7 +614,7 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha upAxisMat.setPureRotation(upAxisTrans.getRotation()); btMatrix4x4 unitMeterScalingMat; unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; + btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat; //btMatrix4x4 worldMat = instance->m_worldTransform; int curNumVertices = glmesh->m_vertices->size(); int additionalVertices = verts.size(); @@ -741,8 +756,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { - - + btAlignedObjectArray vertices; @@ -758,14 +772,9 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy btTransform childTrans; childTrans.setOrigin(childPos); childTrans.setRotation(childOrn); - if (1)//!mappings.m_createMultiBody) - { - convertURDFToVisualShape(vis, pathPrefix, childTrans*inertialFrame.inverse(), vertices, indices); - } - else - { - convertURDFToVisualShape(vis, pathPrefix, childTrans, vertices, indices); - } + + convertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices); + } if (vertices.size() && indices.size()) @@ -773,6 +782,8 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy graphicsIndex = gfxBridge.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++) @@ -786,13 +797,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy btTransform childTrans; childTrans.setOrigin(childPos); childTrans.setRotation(childOrn); - if (1)//!mappings.m_createMultiBody) - { - compoundShape->addChildShape(childTrans*inertialFrame.inverse(),childShape); - } else - { - compoundShape->addChildShape(childTrans,childShape); - } + compoundShape->addChildShape(inertialFrame.inverse()*childTrans,childShape); } }