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

Only support btMultiBody multi-dof version (remove non-multi-dof path)

Browse Source 
Use ATTRIBUTE_ALIGNED16 for btMultiBody
Always disable parentCollision for btMultiBody::setupFixed
This commit is contained in:
erwincoumans
2015-11-05 21:17:46 -08:00
parent d6464ce40d
commit 2920d7e61f
16 changed files with 125 additions and 374 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
View File

@@ -15,7 +15,7 @@ public:
///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) = 0;
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) =0;
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) =0;
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0;

4
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -18,12 +18,12 @@ m_guiHelper(guiHelper)
}
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof)
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)
{
// m_urdf2mbLink.resize(totalNumJoints+1,-2);
m_mb2urdfLink.resize(totalNumJoints+1,-2);
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof);
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep);
return m_bulletMultiBody;
}

2
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
View File

@@ -43,7 +43,7 @@ public:
///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);
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof);
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);

6
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -241,11 +241,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
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.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep);
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
}
@@ -271,7 +271,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
//b3Printf("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);
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin());
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);

13
examples/MultiBody/InvertedPendulumPDControl.cpp
View File

@@ -136,8 +136,8 @@ void InvertedPendulumPDControl::initPhysics()
delete shape;
}
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep, isMultiDof);
btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -201,7 +201,7 @@ void InvertedPendulumPDControl::initPhysics()
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -249,10 +249,9 @@ void InvertedPendulumPDControl::initPhysics()
{
btScalar q0 = 180.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
{
mbC->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);

13
examples/MultiBody/MultiBodyConstraintFeedback.cpp
View File

@@ -131,8 +131,8 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
delete shape;
}
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -196,7 +196,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -244,10 +244,9 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
{
mbC->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);

11
examples/MultiBody/MultiDofDemo.cpp
View File

@@ -161,10 +161,9 @@ void MultiDofDemo::initPhysics()
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
{
mbC->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
@@ -260,8 +259,8 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD
}
bool canSleep = false;
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);

13
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -135,8 +135,8 @@ void TestJointTorqueSetup::initPhysics()
delete shape;
}
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -200,7 +200,7 @@ void TestJointTorqueSetup::initPhysics()
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -248,10 +248,9 @@ void TestJointTorqueSetup::initPhysics()
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
{
mbC->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);