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

more work-in-progress on joint-torque sensor sample and server/client shared memory API

Browse Source
This commit is contained in:
erwin coumans
2015-06-22 15:30:57 -07:00
parent 4688540a98
commit b14afba350
6 changed files with 260 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1027,8 +1027,10 @@ 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[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
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;
}
}

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

@@ -858,6 +858,15 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);