AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

restore multibody world transform and joint angles/velocities from a .bullet file.

Browse Source
This commit is contained in:
Erwin Coumans 2017-12-22 14:45:36 -08:00
parent 41f9bb89e5
commit 799d030874
3 changed files with 33 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp
View File

@ -52,6 +52,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
baseInertia.deSerializeDouble(mbd->m_baseInertia);
btMultiBody* mb = new btMultiBody(mbd->m_numLinks,mbd->m_baseMass,baseInertia,isFixedBase,canSleep);
mb->setHasSelfCollision(false);
btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
mb->setBaseWorldTransform(tr);
m_data->m_mbMap.insert(mbd,mb);
for (int i=0;i<mbd->m_numLinks;i++)
@ -84,6 +86,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
bool disableParentCollision = true;//todo
mb->setupPrismatic(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
parentRotToThis,jointAxis, parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eRevolute:
@ -93,6 +97,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
bool disableParentCollision = true;//todo
mb->setupRevolute(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
parentRotToThis, jointAxis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eSpherical:
@ -101,6 +107,9 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
bool disableParentCollision = true;//todo
mb->setupSpherical(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
parentRotToThis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
mb->setJointPosMultiDof(i, mbd->m_links[i].m_jointPos);
mb->setJointVelMultiDof(i, mbd->m_links[i].m_jointVel);
break;
}
case btMultibodyLink::ePlanar:
@ -117,6 +126,22 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
}
}
//forward kinematics, so that the link world transforms are valid, for collision detection
btAlignedObjectArray<btQuaternion> scratchQ;
btAlignedObjectArray<btVector3> scratchM;
for (int i = 0; i<m_data->m_mbMap.size(); i++)
{
btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
if (ptr)
{
btMultiBody* mb = *ptr;
mb->finalizeMultiDof();
btVector3 linvel = mb->getBaseVel();
btVector3 angvel = mb->getBaseOmega();
mb->forwardKinematics(scratchQ, scratchM);
}
}
//convert all multibody link colliders
for (int i=0;i<bulletFile2->m_multiBodyLinkColliders.size();i++)
{
@ -142,16 +167,17 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
{
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
col->setCollisionShape( shape );
//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
//m_bodyMap.insert(colObjData,body);
if (mblcd->m_link==-1)
{
col->setWorldTransform(multiBody->getBaseWorldTransform());
multiBody->setBaseCollider(col);
} else
{
col->setWorldTransform(multiBody->getLink(mblcd->m_link).m_cachedWorldTransform);
multiBody->getLink(mblcd->m_link).m_collider = col;
}
int mbLinkIndex = mblcd->m_link;

4
data/cube_small.urdf
View File

@ -2,8 +2,8 @@
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<inertia_scaling value="3.0"/>
<friction_anchor/>
<lateral_friction value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>

4
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@ -1236,6 +1236,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
break;
}
case CMD_BULLET_SAVING_COMPLETED:
{
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);