AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #324 from erwincoumans/master

Browse Source 
improved URDF support and fixes in btMultiBody
This commit is contained in:
erwincoumans 2015-02-13 19:04:34 -08:00
commit 2a0787907b
5 changed files with 87 additions and 44 deletions
Show Stats 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)
{
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

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");
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<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;
@ -543,7 +579,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
upAxisTransform.setIdentity();
//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;
ident.setIdentity();

3
Demos3/ImportColladaDemo/LoadMeshFromCollada.h
View File

@ -29,7 +29,8 @@ void LoadMeshFromCollada(const char* relativeFileName,
btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
btTransform& upAxisTrans,
float& unitMeterScaling);
float& unitMeterScaling,
int clientUpAxis);
//#define 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;
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<ColladaGraphicsInstance> 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<ColladaGraphicsInstance> 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<const Link> link, GraphicsPhy
{
btAlignedObjectArray<GLInstanceVertex> vertices;
@ -758,14 +772,9 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> 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<const Link> 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<const Link> 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);
}
}

2
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@ -1265,7 +1265,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
//
//p += vhat x Ihat vhat - done in a simpler way
if(m_useGyroTerm)
zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular()));
zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
//
zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());