Skip to content

Commit

Permalink
move global from btMultiBody into dynamicsWorld.getSolverInfo
Browse files Browse the repository at this point in the history
  • Loading branch information
Erwin Coumans committed Nov 5, 2018
1 parent ac18c95 commit 882252f
Show file tree
Hide file tree
Showing 9 changed files with 64 additions and 48 deletions.
2 changes: 1 addition & 1 deletion Extras/InverseDynamics/invdyn_bullet_comparison.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
btmb->forwardKinematics(world_to_local, local_origin);
btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass);
btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);

// read generalized accelerations back from btMultiBody
// the mapping from scratch variables to accelerations is taken from the implementation
Expand Down
9 changes: 4 additions & 5 deletions examples/MultiBody/InvertedPendulumPDControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,6 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl()
{
}

///this is a temporary global, until we determine if we need the option or not
extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame;

btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans, bool fixedBase)
{
Expand Down Expand Up @@ -315,8 +312,10 @@ void InvertedPendulumPDControl::initPhysics()
}

int upAxis = 1;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;

m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true;
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true;


m_guiHelper->setUpAxis(upAxis);

Expand Down
9 changes: 4 additions & 5 deletions examples/MultiBody/MultiBodyConstraintFeedback.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,14 +44,9 @@ MultiBodyConstraintFeedbackSetup::~MultiBodyConstraintFeedbackSetup()
{
}

///this is a temporary global, until we determine if we need the option or not
extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame;
void MultiBodyConstraintFeedbackSetup::initPhysics()
{
int upAxis = 2;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;
m_guiHelper->setUpAxis(upAxis);

btVector4 colors[4] =
Expand All @@ -69,6 +64,10 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);


m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true;
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true;

//create a static ground object
if (1)
{
Expand Down
9 changes: 4 additions & 5 deletions examples/MultiBody/TestJointTorqueSetup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,15 +44,10 @@ TestJointTorqueSetup::~TestJointTorqueSetup()
{
}

///this is a temporary global, until we determine if we need the option or not
extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame;

void TestJointTorqueSetup::initPhysics()
{
int upAxis = 1;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;

m_guiHelper->setUpAxis(upAxis);

Expand All @@ -71,6 +66,10 @@ void TestJointTorqueSetup::initPhysics()
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);

m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true;
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true;


//create a static ground object
if (1)
{
Expand Down
10 changes: 4 additions & 6 deletions examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,8 +96,6 @@
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#endif

extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame;

int gInternalSimFlags = 0;
bool gResetSimulation = 0;
Expand Down Expand Up @@ -7697,11 +7695,11 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2];
serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags;
serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0;
if (gJointFeedbackInWorldSpace)
if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace)
{
serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_WORLD_SPACE;
}
if (gJointFeedbackInJointFrame)
if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame)
{
serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_JOINT_FRAME;
}
Expand Down Expand Up @@ -7747,8 +7745,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st

if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE)
{
gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0;
gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0;
m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0;
m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0;
}

if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME)
Expand Down
4 changes: 4 additions & 0 deletions src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@ struct btContactSolverInfoData
btScalar m_singleAxisRollingFrictionThreshold;
btScalar m_leastSquaresResidualThreshold;
btScalar m_restitutionVelocityThreshold;
bool m_jointFeedbackInWorldSpace;
bool m_jointFeedbackInJointFrame;
};

struct btContactSolverInfo : public btContactSolverInfoData
Expand Down Expand Up @@ -94,6 +96,8 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_singleAxisRollingFrictionThreshold = 1e30f; ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
m_leastSquaresResidualThreshold = 0.f;
m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution
m_jointFeedbackInWorldSpace = false;
m_jointFeedbackInJointFrame = false;
}
};

Expand Down
17 changes: 8 additions & 9 deletions src/BulletDynamics/Featherstone/btMultiBody.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,6 @@
//#include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM

///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
bool gJointFeedbackInWorldSpace = false;
bool gJointFeedbackInJointFrame = false;

namespace
{
Expand Down Expand Up @@ -718,10 +715,12 @@ inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //ren
//

void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass)
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass,
bool jointFeedbackInWorldSpace,
bool jointFeedbackInJointFrame)
{
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
// and the base linear & angular accelerations.
Expand Down Expand Up @@ -1124,15 +1123,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;

if (gJointFeedbackInJointFrame)
if (jointFeedbackInJointFrame)
{
//shift the reaction forces to the joint frame
//linear (force) component is the same
//shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector
angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
}

if (gJointFeedbackInWorldSpace)
if (jointFeedbackInWorldSpace)
{
if (isConstraintPass)
{
Expand Down
21 changes: 12 additions & 9 deletions src/BulletDynamics/Featherstone/btMultiBody.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -338,17 +338,20 @@ btMultiBody
btAlignedObjectArray<btScalar> & scratch_r,
btAlignedObjectArray<btVector3> & scratch_v,
btAlignedObjectArray<btMatrix3x3> & scratch_m,
bool isConstraintPass = false);
bool isConstraintPass,
bool jointFeedbackInWorldSpace,
bool jointFeedbackInJointFrame
);

///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
void stepVelocitiesMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> & scratch_r,
btAlignedObjectArray<btVector3> & scratch_v,
btAlignedObjectArray<btMatrix3x3> & scratch_m,
bool isConstraintPass = false)
{
computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass);
}
//void stepVelocitiesMultiDof(btScalar dt,
// btAlignedObjectArray<btScalar> & scratch_r,
// btAlignedObjectArray<btVector3> & scratch_v,
// btAlignedObjectArray<btMatrix3x3> & scratch_m,
// bool isConstraintPass = false)
//{
// computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
//}

// calcAccelerationDeltasMultiDof
// input: force vector (in same format as jacobian, i.e.:
Expand Down
31 changes: 23 additions & 8 deletions src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -491,11 +491,14 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
m_scratch_v.resize(bod->getNumLinks() + 1);
m_scratch_m.resize(bod->getNumLinks() + 1);
bool doNotUpdatePos = false;

bool isConstraintPass = false;
{
if (!bod->isUsingRK4Integration())
{
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
else
{
Expand Down Expand Up @@ -593,7 +596,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
btScalar h = solverInfo.m_timeStep;
#define output &m_scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
pCopy(output, scratch_qdd0, 0, numDofs);
//calc q1 = q0 + h/2 * qd0
pResetQx();
Expand All @@ -603,7 +608,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_qd1);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
pCopy(output, scratch_qdd1, 0, numDofs);
//calc q2 = q0 + h/2 * qd1
pResetQx();
Expand All @@ -613,7 +620,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_qd2);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2
pResetQx();
Expand All @@ -623,7 +632,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_qd3);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
pCopy(output, scratch_qdd3, 0, numDofs);

//
Expand Down Expand Up @@ -660,7 +671,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
for (int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof();
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
}
}
Expand Down Expand Up @@ -708,7 +721,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
}
}
Expand Down

0 comments on commit 882252f

Please sign in to comment.