Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix in URDF loading: apply visual transform locally, not after compensating for inertia transform.

Browse Source 
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)
This commit is contained in:
Erwin Coumans
2015-02-13 16:18:16 -08:00
parent a65d8fc209
commit d8b08eaf2a
4 changed files with 86 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Demos3/ImportColladaDemo/ImportColladaSetup.cpp
View File

@@ -46,7 +46,8 @@ static int ColladaGraphicsInstanceSortfnc(const ColladaGraphicsInstance& a,const
void ImportColladaSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) void ImportColladaSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{ {
gfxBridge.setUpAxis(1); int upAxis=1;
gfxBridge.setUpAxis(upAxis);
this->createEmptyDynamicsWorld(); this->createEmptyDynamicsWorld();
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
@@ -112,7 +113,7 @@ void ImportColladaSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{ {
fileIndex = 0; fileIndex = 0;
} }
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling); LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling, upAxis);
#endif// COMPARE_WITH_ASSIMP #endif// COMPARE_WITH_ASSIMP

70
Demos3/ImportColladaDemo/LoadMeshFromCollada.cpp
View File

@@ -485,8 +485,10 @@ void readVisualSceneInstanceGeometries(TiXmlDocument& doc, btHashMap<btHashStrin
} }
} }
void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr, float& unitMeterScaling) void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr, float& unitMeterScaling, int clientUpAxis)
{ {
///todo(erwincoumans) those up-axis transformations have been quickly coded without rigorous testing
TiXmlElement* unitMeter = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("unit"); TiXmlElement* unitMeter = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("unit");
if (unitMeter) if (unitMeter)
{ {
@@ -498,25 +500,59 @@ void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr,
TiXmlElement* upAxisElem = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("up_axis"); TiXmlElement* upAxisElem = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("up_axis");
if (upAxisElem) if (upAxisElem)
{ {
std::string upAxisTxt = upAxisElem->GetText(); switch (clientUpAxis)
if (upAxisTxt == "X_UP")
{ {
btQuaternion y2x(btVector3(0,0,1),SIMD_HALF_PI);
tr.setRotation(y2x); case 1:
} {
if (upAxisTxt == "Y_UP") std::string upAxisTxt = upAxisElem->GetText();
{ if (upAxisTxt == "X_UP")
//assume Y_UP for now, to be compatible with assimp? {
} btQuaternion x2y(btVector3(0,0,1),SIMD_HALF_PI);
if (upAxisTxt == "Z_UP") tr.setRotation(x2y);
{ }
btQuaternion y2z(btVector3(1,0,0),-SIMD_HALF_PI); if (upAxisTxt == "Y_UP")
tr.setRotation(y2z); {
} //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<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling) void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling,int clientUpAxis)
{ {
GLInstanceGraphicsShape* instance = 0; GLInstanceGraphicsShape* instance = 0;
@@ -543,7 +579,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
upAxisTransform.setIdentity(); upAxisTransform.setIdentity();
//Also we can optionally compensate all transforms using the asset/up_axis as well as unit meter scaling //Also we can optionally compensate all transforms using the asset/up_axis as well as unit meter scaling
getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling); getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling,clientUpAxis);
btMatrix4x4 ident; btMatrix4x4 ident;
ident.setIdentity(); ident.setIdentity();

3
Demos3/ImportColladaDemo/LoadMeshFromCollada.h
View File

@@ -29,7 +29,8 @@ void LoadMeshFromCollada(const char* relativeFileName,
btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
btTransform& upAxisTrans, btTransform& upAxisTrans,
float& unitMeterScaling); float& unitMeterScaling,
int clientUpAxis);
//#define COMPARE_WITH_ASSIMP //#define COMPARE_WITH_ASSIMP
#ifdef COMPARE_WITH_ASSIMP #ifdef COMPARE_WITH_ASSIMP

51
Demos3/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -13,10 +13,21 @@ static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphasePr
static bool enableConstraints = true;//false; static bool enableConstraints = true;//false;
const char* fileNames[] =
{
"r2d2.urdf",
};
ImportUrdfSetup::ImportUrdfSetup() 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() ImportUrdfSetup::~ImportUrdfSetup()
@@ -261,12 +272,14 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances; btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
btTransform upAxisTrans; upAxisTrans.setIdentity(); btTransform upAxisTrans; upAxisTrans.setIdentity();
float unitMeterScaling = 1; float unitMeterScaling = 1;
int upAxis = 2;
LoadMeshFromCollada(fullPath, LoadMeshFromCollada(fullPath,
visualShapes, visualShapes,
visualShapeInstances, visualShapeInstances,
upAxisTrans, upAxisTrans,
unitMeterScaling); unitMeterScaling,
upAxis);
glmesh = new GLInstanceGraphicsShape; glmesh = new GLInstanceGraphicsShape;
int index = 0; int index = 0;
@@ -307,7 +320,8 @@ void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, cons
//compensate upAxisTrans and unitMeterScaling here //compensate upAxisTrans and unitMeterScaling here
btMatrix4x4 upAxisMat; btMatrix4x4 upAxisMat;
upAxisMat.setPureRotation(upAxisTrans.getRotation()); upAxisMat.setIdentity();
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
btMatrix4x4 unitMeterScalingMat; btMatrix4x4 unitMeterScalingMat;
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
@@ -550,12 +564,13 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances; btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
btTransform upAxisTrans;upAxisTrans.setIdentity(); btTransform upAxisTrans;upAxisTrans.setIdentity();
float unitMeterScaling=1; float unitMeterScaling=1;
int upAxis = 2;
LoadMeshFromCollada(fullPath, LoadMeshFromCollada(fullPath,
visualShapes, visualShapes,
visualShapeInstances, visualShapeInstances,
upAxisTrans, upAxisTrans,
unitMeterScaling); unitMeterScaling,
upAxis );
glmesh = new GLInstanceGraphicsShape; glmesh = new GLInstanceGraphicsShape;
int index = 0; int index = 0;
@@ -599,7 +614,7 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
upAxisMat.setPureRotation(upAxisTrans.getRotation()); upAxisMat.setPureRotation(upAxisTrans.getRotation());
btMatrix4x4 unitMeterScalingMat; btMatrix4x4 unitMeterScalingMat;
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); 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; //btMatrix4x4 worldMat = instance->m_worldTransform;
int curNumVertices = glmesh->m_vertices->size(); int curNumVertices = glmesh->m_vertices->size();
int additionalVertices = verts.size(); int additionalVertices = verts.size();
@@ -741,8 +756,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
{ {
btAlignedObjectArray<GLInstanceVertex> vertices; btAlignedObjectArray<GLInstanceVertex> vertices;
@@ -758,14 +772,9 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
btTransform childTrans; btTransform childTrans;
childTrans.setOrigin(childPos); childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn); childTrans.setRotation(childOrn);
if (1)//!mappings.m_createMultiBody)
{ convertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
convertURDFToVisualShape(vis, pathPrefix, childTrans*inertialFrame.inverse(), vertices, indices);
}
else
{
convertURDFToVisualShape(vis, pathPrefix, childTrans, vertices, indices);
}
} }
if (vertices.size() && indices.size()) if (vertices.size() && indices.size())
@@ -773,6 +782,8 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
graphicsIndex = gfxBridge.registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); graphicsIndex = gfxBridge.registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
} }
btCompoundShape* compoundShape = new btCompoundShape(); btCompoundShape* compoundShape = new btCompoundShape();
compoundShape->setMargin(0.001); compoundShape->setMargin(0.001);
for (int v=0;v<(int)link->collision_array.size();v++) for (int v=0;v<(int)link->collision_array.size();v++)
@@ -786,13 +797,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
btTransform childTrans; btTransform childTrans;
childTrans.setOrigin(childPos); childTrans.setOrigin(childPos);
childTrans.setRotation(childOrn); childTrans.setRotation(childOrn);
if (1)//!mappings.m_createMultiBody) compoundShape->addChildShape(inertialFrame.inverse()*childTrans,childShape);
{
compoundShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
} else
{
compoundShape->addChildShape(childTrans,childShape);
}
} }
} }