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

add option for rigid body/typed constraint to set target velocity

Browse Source 
compare joint feedback between multi body and rigid body. initial results are promising (not exactly the same, but reasonably close)
This commit is contained in:
Erwin Coumans
2015-06-24 23:19:00 -07:00
parent b14afba350
commit d830681674
11 changed files with 426 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -30,6 +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
bool gJointFeedbackInWorldSpace = false;
namespace {
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
@@ -1026,12 +1029,17 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
m_internalNeedsJointFeedback = true;
m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
//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_spatialInertia = spatInertia[i+1];
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;
} 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;
}
}
}

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

@@ -616,7 +616,6 @@ private:
btAlignedObjectArray<btVector3> m_vectorBuf;
btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
btMatrix3x3 m_cachedInertiaTopLeft;
btMatrix3x3 m_cachedInertiaTopRight;

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

@@ -22,7 +22,6 @@ subject to the following restrictions:
struct btMultiBodyJointFeedback
{
btSpatialForceVector m_reactionForces;
btSymmetricSpatialDyad m_spatialInertia;
};
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H