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

expose btMultiBodyConstraint applied impulse (force) on its degree of freedom(s),

Browse Source 
only tested for btMultiBodyJointMotor for now.
See also MultiBody/MultiBodyConstraintFeedback example
This commit is contained in:
=
2015-07-06 23:00:46 -07:00
parent 33b0d429ba
commit 3431773800
10 changed files with 50 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -58,6 +58,7 @@ protected:
btScalar m_maxAppliedImpulse;
// warning: the data block lay out is not consistent for all constraints
// data block laid out as follows:
// cached impulses. (one per row.)
// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
@@ -108,6 +109,19 @@ public:
return m_bodyB;
}
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
{
btAssert(dof>=0);
btAssert(dof < getNumRows());
m_data[dof] = appliedImpulse;
}
btScalar getAppliedImpulse(int dof)
{
btAssert(dof>=0);
btAssert(dof < getNumRows());
return m_data[dof];
}
// current constraint position
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
// NOTE: ignored position for friction rows.

12
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -612,7 +612,9 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
{
BT_PROFILE("addMultiBodyFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
solverConstraint.m_useJointForce = false;
solverConstraint.m_orgConstraint = 0;
solverConstraint.m_orgDofIndex = -1;
solverConstraint.m_frictionIndex = frictionIndex;
bool isFriction = true;
@@ -680,10 +682,11 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
int frictionIndex = m_multiBodyNormalContactConstraints.size();
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
solverConstraint.m_useJointForce = false;
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint.m_orgConstraint = 0;
solverConstraint.m_orgDofIndex = -1;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_multiBodyA = mbA;
@@ -891,6 +894,11 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
//bod->addBaseForce(m_gravity * bod->getBaseMass());
//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
if (c.m_orgConstraint)
{
c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
}
if (c.m_multiBodyA)

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

@@ -114,6 +114,9 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
btScalar direction = row? -1 : 1;
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
const btScalar posError = 0; //why assume it's zero?
@@ -123,7 +126,6 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
if (m_bodyA->isMultiDof())
{
constraintRow.m_useJointForce = false;
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)

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

@@ -116,10 +116,10 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
if (m_bodyA->isMultiDof())
{
constraintRow.m_useJointForce = false;
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)

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

@@ -33,6 +33,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
@@ -42,6 +43,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, bt
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodyPoint2Point::finalizeMultiDof()
@@ -108,7 +110,8 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
constraintRow.m_useJointForce = false;
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);

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

@@ -20,6 +20,7 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
class btMultiBody;
class btMultiBodyConstraint;
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
@@ -28,7 +29,7 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_useJointForce(false)
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
{}
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
@@ -73,7 +74,9 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
btMultiBody* m_multiBodyB;
int m_linkB;
bool m_useJointForce;//needed for write-back of joint versus link/base force/torque
//for writing back applied impulses
btMultiBodyConstraint* m_orgConstraint;
int m_orgDofIndex;
enum btSolverConstraintType
{