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

preliminary work towards saveState/restoreState and saveRestoreState.py example (not implemented yet)

Browse Source 
allow multiple options in connect, for example: p.connect(p.GUI, options="--width=640, --height=480")
This commit is contained in:
Erwin Coumans
2017-12-28 10:05:51 -08:00
parent 799d030874
commit 97547eda0d
15 changed files with 552 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -36,45 +36,87 @@ void btMultiBodyWorldImporter::deleteAllData()
bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile2)
{
bool result = btBulletWorldImporter::convertAllObjects(bulletFile2);
//convert all multibodies
for (int i=0;i<bulletFile2->m_multiBodies.size();i++)
bool result = false;
btAlignedObjectArray<btQuaternion> scratchQ;
btAlignedObjectArray<btVector3> scratchM;
if (m_importerFlags&eRESTORE_EXISTING_OBJECTS)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
//check if the snapshot is valid for the existing world
//equal number of objects, # links etc
if (bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies())
{
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*) bulletFile2->m_multiBodies[i];
bool isFixedBase = mbd->m_baseMass==0;
bool canSleep = false;
btVector3 baseInertia;
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);
result = false;
return result;
}
result = true;
m_data->m_mbMap.insert(mbd,mb);
for (int i=0;i<mbd->m_numLinks;i++)
//convert all multibodies
for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btVector3 localInertiaDiagonal;
localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
btQuaternion parentRotToThis;
parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
btVector3 parentComToThisPivotOffset;
parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
btVector3 thisPivotToThisComOffset;
thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
bool isFixedBase = mbd->m_baseMass == 0;
bool canSleep = false;
btVector3 baseInertia;
baseInertia.deSerializeDouble(mbd->m_baseInertia);
switch (mbd->m_links[i].m_jointType)
btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
mb->setBaseWorldTransform(tr);
for (int i = 0; i < mbd->m_numLinks; i++)
{
}
btVector3 linvel = mb->getBaseVel();
btVector3 angvel = mb->getBaseOmega();
mb->forwardKinematics(scratchQ, scratchM);
mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
}
}
}
else
{
result = btBulletWorldImporter::convertAllObjects(bulletFile2);
//convert all multibodies
for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
bool isFixedBase = mbd->m_baseMass == 0;
bool canSleep = false;
btVector3 baseInertia;
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++)
{
btVector3 localInertiaDiagonal;
localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
btQuaternion parentRotToThis;
parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
btVector3 parentComToThisPivotOffset;
parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
btVector3 thisPivotToThisComOffset;
thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
switch (mbd->m_links[i].m_jointType)
{
case btMultibodyLink::eFixed:
{
mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, parentComToThisPivotOffset,thisPivotToThisComOffset);
parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
//search for the collider
//mbd->m_links[i].m_linkCollider
break;
@@ -84,8 +126,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
btVector3 jointAxis;
jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisBottom[0]);
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->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;
@@ -95,8 +137,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
btVector3 jointAxis;
jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisTop[0]);
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->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;
@@ -105,8 +147,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
{
btAssert(0);
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->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);
@@ -121,109 +163,110 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
{
btAssert(0);
}
}
}
}
}
}
//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++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
//forward kinematics, so that the link world transforms are valid, for collision detection
for (int i = 0; i < m_data->m_mbMap.size(); i++)
{
btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*) bulletFile2->m_multiBodyLinkColliders[i];
btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
if (ptr)
{
btMultiBody* multiBody = *ptr;
btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
if (shapePtr && *shapePtr)
{
btTransform startTransform;
mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
if (shape)
{
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;
bool isDynamic = (mbLinkIndex<0 && multiBody->hasFixedBase())? false : true;
int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
#if 0
int colGroup=0, colMask=0;
int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
if (collisionFlags & URDF_HAS_COLLISION_GROUP)
{
collisionFilterGroup = colGroup;
}
if (collisionFlags & URDF_HAS_COLLISION_MASK)
{
collisionFilterMask = colMask;
}
#endif
m_data->m_mbDynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
}
} else
{
printf("error: no shape found\n");
}
#if 0
//base and fixed? -> static, otherwise flag as dynamic
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
#endif
btMultiBody* mb = *ptr;
mb->finalizeMultiDof();
btVector3 linvel = mb->getBaseVel();
btVector3 angvel = mb->getBaseOmega();
mb->forwardKinematics(scratchQ, scratchM);
}
}
}
for (int i=0;i<m_data->m_mbMap.size();i++)
{
btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
if (ptr)
//convert all multibody link colliders
for (int i = 0; i < bulletFile2->m_multiBodyLinkColliders.size(); i++)
{
btMultiBody* mb = *ptr;
mb->finalizeMultiDof();
m_data->m_mbDynamicsWorld->addMultiBody(mb);
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*)bulletFile2->m_multiBodyLinkColliders[i];
btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
if (ptr)
{
btMultiBody* multiBody = *ptr;
btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
if (shapePtr && *shapePtr)
{
btTransform startTransform;
mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
if (shape)
{
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;
bool isDynamic = (mbLinkIndex < 0 && multiBody->hasFixedBase()) ? false : true;
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
#if 0
int colGroup = 0, colMask = 0;
int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
if (collisionFlags & URDF_HAS_COLLISION_GROUP)
{
collisionFilterGroup = colGroup;
}
if (collisionFlags & URDF_HAS_COLLISION_MASK)
{
collisionFilterMask = colMask;
}
#endif
m_data->m_mbDynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
}
}
else
{
printf("error: no shape found\n");
}
#if 0
//base and fixed? -> static, otherwise flag as dynamic
world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
#endif
}
}
}
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();
m_data->m_mbDynamicsWorld->addMultiBody(mb);
}
}
}
return result;

3
Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp
View File

@@ -20,7 +20,8 @@ subject to the following restrictions:
#endif
btWorldImporter::btWorldImporter(btDynamicsWorld* world)
:m_dynamicsWorld(world),
m_verboseMode(0)
m_verboseMode(0),
m_importerFlags(0)
{
}

17
Extras/Serialize/BulletWorldImporter/btWorldImporter.h
View File

@@ -59,6 +59,10 @@ struct btRigidBodyFloatData;
#define btRigidBodyData btRigidBodyFloatData
#endif//BT_USE_DOUBLE_PRECISION
enum btWorldImporterFlags
{
eRESTORE_EXISTING_OBJECTS=1,//don't create new objects
};
class btWorldImporter
{
@@ -66,6 +70,7 @@ protected:
btDynamicsWorld* m_dynamicsWorld;
int m_verboseMode;
int m_importerFlags;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
@@ -131,6 +136,18 @@ public:
return m_verboseMode;
}
void setImporterFlags(int importerFlags)
{
m_importerFlags = importerFlags;
}
int getImporterFlags() const
{
return m_importerFlags;
}
// query for data
int getNumCollisionShapes() const;
btCollisionShape* getCollisionShapeByIndex(int index);