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

Improved URDF support for btMultiBody and separate graphics/collision/inertial frames and shapes

Browse Source 
Fix WinXP GetTickCount64 with a typedef
Expose debug drawing mode/flags in UI (hot keys A,D,L,W for now, buttons later)
GLInstancingRenderer: tweak near/far planes to allow closer approach of camera
btDiscreteDynamicsWorld: enable debug drawing for btGeneric6DofSpring2Constraint
btMultiBodyDynamicsWorld: enable basic debug drawing for btMultiBody
btMultibody: allow center-of-mass shift for prismatic and fixed constraint
This commit is contained in:
erwincoumans
2015-01-22 17:56:24 -08:00
parent 6e70f285e1
commit a159fbac69
28 changed files with 1022 additions and 674 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -137,16 +137,18 @@ void btMultiBody::setupFixed(int i,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision)
{
btAssert(m_isMultiDof);
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_dVector = thisPivotToThisComOffset;
m_links[i].m_eVector = parentComToThisPivotOffset;
m_links[i].m_jointType = btMultibodyLink::eFixed;
m_links[i].m_dofCount = 0;
@@ -156,6 +158,7 @@ void btMultiBody::setupFixed(int i,
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
//
m_links[i].updateCacheMultiDof();
//
//if(m_isMultiDof)
// resizeInternalMultiDofBuffers();
@@ -180,12 +183,12 @@ void btMultiBody::setupPrismatic(int i,
}
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].setAxisTop(0, 0., 0., 0.);
m_links[i].setAxisBottom(0, jointAxis);
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_cachedRotParentToThis = rotParentToThis;
m_links[i].m_jointType = btMultibodyLink::ePrismatic;
@@ -223,7 +226,7 @@ void btMultiBody::setupRevolute(int i,
}
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].setAxisTop(0, jointAxis);
@@ -265,7 +268,7 @@ void btMultiBody::setupSpherical(int i,
m_posVarCnt += 4;
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_dVector = thisPivotToThisComOffset;
@@ -307,7 +310,7 @@ void btMultiBody::setupPlanar(int i,
m_posVarCnt += 3;
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_dVector.setZero();
@@ -365,7 +368,7 @@ btScalar btMultiBody::getLinkMass(int i) const
const btVector3 & btMultiBody::getLinkInertia(int i) const
{
return m_links[i].m_inertia;
return m_links[i].m_inertiaLocal;
}
btScalar btMultiBody::getJointPos(int i) const
@@ -506,7 +509,7 @@ btScalar btMultiBody::getKineticEnergy() const
for (int i = 0; i < num_links; ++i) {
result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
result += omega[i+1].dot(m_links[i].m_inertia * omega[i+1]);
result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
}
return 0.5f * result;
@@ -526,7 +529,7 @@ btVector3 btMultiBody::getAngularMomentum() const
for (int i = 0; i < num_links; ++i) {
rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertia * omega[i+1])));
result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
}
return result;
@@ -1241,7 +1244,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
//
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
// calculate Ihat_i^A
@@ -1252,9 +1255,9 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
0, m_links[i].m_mass, 0,
0, 0, m_links[i].m_mass),
//
btMatrix3x3(m_links[i].m_inertia[0], 0, 0,
0, m_links[i].m_inertia[1], 0,
0, 0, m_links[i].m_inertia[2])
btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
0, m_links[i].m_inertiaLocal[1], 0,
0, 0, m_links[i].m_inertiaLocal[2])
);
//
//p += vhat x Ihat vhat - done in a simpler way
@@ -1694,19 +1697,19 @@ void btMultiBody::stepVelocities(btScalar dt,
- (rot_from_world[i+1] * m_links[i].m_appliedTorque);
if (m_useGyroTerm)
{
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] );
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
}
zero_acc_bottom_linear[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
// calculate Ihat_i^A
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
0, m_links[i].m_mass, 0,
0, 0, m_links[i].m_mass);
inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0,
0, m_links[i].m_inertia[1], 0,
0, 0, m_links[i].m_inertia[2]);
inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
0, m_links[i].m_inertiaLocal[1], 0,
0, 0, m_links[i].m_inertiaLocal[2]);
}

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

@@ -61,7 +61,8 @@ public:
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision);

80
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"
#include "LinearMath/btIDebugDraw.h"
@@ -755,3 +755,81 @@ void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint*
{
m_multiBodyConstraints.remove(constraint);
}
void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
if (drawConstraints)
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
int nLinks = bod->getNumLinks();
///base + num m_links
world_to_local.resize(nLinks + 1);
local_origin.resize(nLinks + 1);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();
{
btVector3 posr = local_origin[0];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
getDebugDrawer()->drawTransform(tr, 0.1);
}
for (int k = 0; k<bod->getNumLinks(); k++)
{
const int parent = bod->getParent(k);
world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
}
for (int m = 0; m<bod->getNumLinks(); m++)
{
int link = m;
int index = link + 1;
btVector3 posr = local_origin[index];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
getDebugDrawer()->drawTransform(tr, 0.1);
}
}
}
}
btDiscreteDynamicsWorld::debugDrawWorld();
}

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

@@ -54,5 +54,7 @@ public:
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
virtual void debugDrawWorld();
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

12
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -355,7 +355,7 @@ struct btMultibodyLink
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar m_mass; // mass of link
btVector3 m_inertia; // inertia of link (local frame; diagonal)
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
@@ -440,15 +440,15 @@ struct btMultibodyLink
btMultibodyLink()
: m_mass(1),
m_parent(-1),
m_zeroRotParentToThis(1, 0, 0, 0),
m_cachedRotParentToThis(1, 0, 0, 0),
m_zeroRotParentToThis(0, 0, 0, 1),
m_cachedRotParentToThis(0, 0, 0, 1),
m_collider(0),
m_flags(0),
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid)
{
m_inertia.setValue(1, 1, 1);
m_inertiaLocal.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
setAxisBottom(0, 1., 0., 0.);
m_dVector.setValue(0, 0, 0);
@@ -493,7 +493,7 @@ struct btMultibodyLink
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_eVector + pJointPos[0] * getAxisBottom(0);
m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
@@ -516,7 +516,7 @@ struct btMultibodyLink
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector);
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}