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

fix an issue with btMultiBodyConstraint, automatically 'finalizeMultiDof' to pre-allocate jacobian data

Browse Source 
enable joint limit for slider/prismatic joint in btMultiBody version of URDF loader
This commit is contained in:
Erwin Coumans
2015-06-05 11:46:53 -07:00
parent a94ac6300a
commit 1a4ce475f7
12 changed files with 307 additions and 249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -248,6 +248,10 @@ void ImportUrdfSetup::initPhysics()
//the btMultiBody support is work-in-progress :-)
for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
{
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
}

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

@@ -391,8 +391,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
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);
world1->addMultiBodyConstraint(con);
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
} else
{

2
examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp
View File

@@ -97,7 +97,7 @@ static void PairKeyboardCallback(int key, int state)
{
if (key=='R' && state)
{
gReset = true;
//gReset = true;
}
//b3DefaultKeyboardCallback(key,state);

14
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -13,12 +13,13 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod
m_jacSizeA(0),
m_jacSizeBoth(0),
m_isUnilateral(isUnilateral),
m_maxAppliedImpulse(100)
m_maxAppliedImpulse(100),
m_numDofsFinalized(-1)
{
finalizeMultiDof();
}
void btMultiBodyConstraint::finalizeMultiDof()
void btMultiBodyConstraint::updateJacobianSizes()
{
if(m_bodyA)
{
@@ -37,6 +38,11 @@ void btMultiBodyConstraint::finalizeMultiDof()
}
else
m_jacSizeBoth = m_jacSizeA;
}
void btMultiBodyConstraint::allocateJacobiansMultiDof()
{
updateJacobianSizes();
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
@@ -63,6 +69,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
btScalar relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;

7
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -54,7 +54,7 @@ protected:
int m_posOffset;
bool m_isUnilateral;
int m_numDofsFinalized;
btScalar m_maxAppliedImpulse;
@@ -82,7 +82,10 @@ public:
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
virtual ~btMultiBodyConstraint();
void finalizeMultiDof();
void updateJacobianSizes();
void allocateJacobiansMultiDof();
virtual void finalizeMultiDof()=0;
virtual int getIslandIdA() const =0;
virtual int getIslandIdB() const =0;

15
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -51,6 +51,21 @@ public:
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual int getNumMultiBodyConstraints() const
{
return m_multiBodyConstraints.size();
}
virtual btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex)
{
return m_multiBodyConstraints[constraintIndex];
}
virtual const btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) const
{
return m_multiBodyConstraints[constraintIndex];
}
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);

19
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
View File

@@ -26,14 +26,18 @@ btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* bo
:btMultiBodyConstraint(body,body,link,link,2,true),
m_lowerBound(lower),
m_upperBound(upper)
{
}
void btMultiBodyJointLimitConstraint::finalizeMultiDof()
{
// the data.m_jacobians never change, so may as well
// initialize them here
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
allocateJacobiansMultiDof();
unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset : m_linkA);
// row 0: the lower bound
jacobianA(0)[offset] = 1;
@@ -41,7 +45,10 @@ btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* bo
//jacobianA(1)[offset] = -1;
jacobianB(1)[offset] = -1;
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
@@ -88,6 +95,12 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
// row 0: the lower bound
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent

2
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
View File

@@ -30,6 +30,8 @@ public:
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
virtual ~btMultiBodyJointLimitConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;

33
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -25,22 +25,29 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
:btMultiBodyConstraint(body,body,link,link,1,true),
m_desiredVelocity(desiredVelocity)
{
int linkDoF = 0;
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
// initialize them here
}
void btMultiBodyJointMotor::finalizeMultiDof()
{
allocateJacobiansMultiDof();
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
int linkDoF = 0;
unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF : m_linkA);
// row 0: the lower bound
// row 0: the lower bound
jacobianA(0)[offset] = 1;
}
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
@@ -50,17 +57,7 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int li
btAssert(linkDoF < body->getLink(link).m_dofCount);
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
// initialize them here
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
// row 0: the lower bound
// row 0: the lower bound
jacobianA(0)[offset] = 1;
}
btMultiBodyJointMotor::~btMultiBodyJointMotor()
{
@@ -102,6 +99,14 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);

1
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
View File

@@ -33,6 +33,7 @@ public:
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
virtual ~btMultiBodyJointMotor();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;

5
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
View File

@@ -44,6 +44,11 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, bt
{
}
void btMultiBodyPoint2Point::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
{

2
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
View File

@@ -39,6 +39,8 @@ public:
virtual ~btMultiBodyPoint2Point();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;