Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests 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
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); baseInertia.deSerializeDouble(mbd->m_baseInertia);
btMultiBody* mb = new btMultiBody(mbd->m_numLinks,mbd->m_baseMass,baseInertia,isFixedBase,canSleep); btMultiBody* mb = new btMultiBody(mbd->m_numLinks,mbd->m_baseMass,baseInertia,isFixedBase,canSleep);
mb->setHasSelfCollision(false); mb->setHasSelfCollision(false);
btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
mb->setBaseWorldTransform(tr);
m_data->m_mbMap.insert(mbd,mb); m_data->m_mbMap.insert(mbd,mb);
for (int i=0;i<mbd->m_numLinks;i++) for (int i=0;i<mbd->m_numLinks;i++)
@@ -84,6 +86,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
bool disableParentCollision = true;//todo bool disableParentCollision = true;//todo
mb->setupPrismatic(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex, mb->setupPrismatic(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
parentRotToThis,jointAxis, parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision); 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; break;
} }
case btMultibodyLink::eRevolute: case btMultibodyLink::eRevolute:
@@ -93,6 +97,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
bool disableParentCollision = true;//todo bool disableParentCollision = true;//todo
mb->setupRevolute(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex, mb->setupRevolute(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
parentRotToThis, jointAxis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision); 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; break;
} }
case btMultibodyLink::eSpherical: case btMultibodyLink::eSpherical:
@@ -101,6 +107,9 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
bool disableParentCollision = true;//todo bool disableParentCollision = true;//todo
mb->setupSpherical(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex, mb->setupSpherical(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
parentRotToThis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision); parentRotToThis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
mb->setJointPosMultiDof(i, mbd->m_links[i].m_jointPos);
mb->setJointVelMultiDof(i, mbd->m_links[i].m_jointVel);
break; break;
} }
case btMultibodyLink::ePlanar: 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 //convert all multibody link colliders
for (int i=0;i<bulletFile2->m_multiBodyLinkColliders.size();i++) 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); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
col->setCollisionShape( shape ); col->setCollisionShape( shape );
//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name); //btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
col->setFriction(btScalar(mblcd->m_colObjData.m_friction)); col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution)); col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
//m_bodyMap.insert(colObjData,body); //m_bodyMap.insert(colObjData,body);
if (mblcd->m_link==-1) if (mblcd->m_link==-1)
{ {
col->setWorldTransform(multiBody->getBaseWorldTransform());
multiBody->setBaseCollider(col); multiBody->setBaseCollider(col);
} else } else
{ {
col->setWorldTransform(multiBody->getLink(mblcd->m_link).m_cachedWorldTransform);
multiBody->getLink(mblcd->m_link).m_collider = col; multiBody->getLink(mblcd->m_link).m_collider = col;
} }
int mbLinkIndex = mblcd->m_link; int mbLinkIndex = mblcd->m_link;

2
data/cube_small.urdf
View File

@@ -2,8 +2,8 @@
<robot name="cube.urdf"> <robot name="cube.urdf">
<link name="baseLink"> <link name="baseLink">
<contact> <contact>
<friction_anchor/>
<lateral_friction value="1.0"/> <lateral_friction value="1.0"/>
<inertia_scaling value="3.0"/>
</contact> </contact>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <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; break;
} }
case CMD_BULLET_SAVING_COMPLETED:
{
break;
}
default: { default: {
b3Error("Unknown server status %d\n", serverCmd.m_type); b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0); btAssert(0);