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

add the option to shift the applied force/torque for a multibody joint (mobilizer) to the joint frame origin

Browse Source
This commit is contained in:
erwin coumans
2015-06-25 15:09:35 -07:00
parent 6a282601cf
commit 3b4ad1cd5a
3 changed files with 22 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/Constraints/TestHingeTorque.cpp
View File

@@ -85,7 +85,7 @@ void TestHingeTorque::stepSimulation(float deltaTime)
for (int i=0;i<m_jointFeedback.size();i++)
{
b3Printf("Applied force B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
b3Printf("Applied force at the COM/Inertial frame B[%d]:(%f,%f,%f), torque B:(%f,%f,%f)\n", i,
m_jointFeedback[i]->m_appliedForceBodyB.x(),
@@ -111,7 +111,7 @@ void TestHingeTorque::initPhysics()
createEmptyDynamicsWorld();
m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
m_dynamicsWorld->setGravity(btVector3(0,-1,-10));
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
int mode = btIDebugDraw::DBG_DrawWireframe

2
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -235,7 +235,7 @@ void TestJointTorqueSetup::initPhysics()
mbC->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0,-1,-10));
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
//////////////////////////////////////////////
if(0)//numLinks > 0)

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

@@ -30,8 +30,9 @@
#include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM
///todo: determine if we need this option. If so, make a proper API, otherwise delete the global
///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
bool gJointFeedbackInWorldSpace = false;
bool gJointFeedbackInJointFrame = false;
namespace {
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
@@ -1026,18 +1027,28 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
if (m_links[i].m_jointFeedback)
{
m_internalNeedsJointFeedback = true;
//m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
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)
{
//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)
{
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
} else
{
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
}
}