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

291
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);
bool result = false;
btAlignedObjectArray<btQuaternion> scratchQ;
btAlignedObjectArray<btVector3> scratchM;
//convert all multibodies
for (int i=0;i<bulletFile2->m_multiBodies.size();i++)
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)
//forward kinematics, so that the link world transforms are valid, for collision detection
for (int i = 0; i < m_data->m_mbMap.size(); i++)
{
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)
{
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();
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*)bulletFile2->m_multiBodyLinkColliders[i];
m_data->m_mbDynamicsWorld->addMultiBody(mb);
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);

12
examples/Importers/ImportURDFDemo/urdfStringSplit.h
View File

@@ -2,14 +2,20 @@
#ifndef STRING_SPLIT_H
#define STRING_SPLIT_H
#ifdef __cplusplus
#include <cstring>
#include <string>
#include "LinearMath/btAlignedObjectArray.h"
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray);
#endif
#ifdef __cplusplus
extern "C" {
#endif
///The string split C code is by Lars Wirzenius
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
@@ -26,5 +32,9 @@ void urdfStrArrayFree(char** array);
/* Return length of a NULL-delimited array of strings. */
size_t urdfStrArrayLen(char** array);
#ifdef __cplusplus
}
#endif
#endif //STRING_SPLIT_H

85
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -107,6 +107,91 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClien
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadStateCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_RESTORE_STATE;
command->m_updateFlags = 0;
command->m_loadStateArguments.m_fileName[0] = 0;
command->m_loadStateArguments.m_stateId = -1;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API int b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_RESTORE_STATE);
if (command->m_type == CMD_RESTORE_STATE)
{
command->m_loadStateArguments.m_stateId = stateId;
command->m_updateFlags |= CMD_LOAD_STATE_HAS_STATEID;
}
return 0;
}
B3_SHARED_API int b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_RESTORE_STATE);
if (command->m_type == CMD_RESTORE_STATE)
{
int len = strlen(fileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_loadStateArguments.m_fileName, fileName);
}
else
{
command->m_loadStateArguments.m_fileName[0] = 0;
}
command->m_updateFlags |= CMD_LOAD_STATE_HAS_FILENAME;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SAVE_STATE;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle)
{
int stateId = -1;
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_SAVE_STATE_COMPLETED);
if (status && status->m_type == CMD_SAVE_STATE_COMPLETED)
{
stateId = status->m_saveStateResultArgs.m_stateId;
}
return stateId;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;

9
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -313,7 +313,16 @@ B3_SHARED_API int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadStateCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId);
B3_SHARED_API int b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);

9
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -1236,6 +1236,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
break;
}
case CMD_RESTORE_STATE_FAILED:
{
b3Warning("restoreState failed");
break;
}
case CMD_RESTORE_STATE_COMPLETED:
{
break;
}
case CMD_BULLET_SAVING_COMPLETED:
{
break;

13
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -1007,6 +1007,19 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
break;
}
case CMD_RESTORE_STATE_FAILED:
{
b3Warning("restoreState failed");
break;
}
case CMD_RESTORE_STATE_COMPLETED:
{
break;
}
case CMD_BULLET_SAVING_COMPLETED:
{
break;
}
default:
{
//b3Warning("Unknown server status type");

62
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -8121,6 +8121,62 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("CMD_RESTORE_STATE");
bool hasStatus = true;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_RESTORE_STATE_FAILED;
btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
importer->setImporterFlags(eRESTORE_EXISTING_OBJECTS);
bool ok = false;
if (clientCmd.m_loadStateArguments.m_stateId >= 0)
{
char* memoryBuffer = 0;
int len = 0;
ok = importer->loadFileFromMemory(memoryBuffer, len);
}
else
{
const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" };
int numPrefixes = sizeof(prefix) / sizeof(const char*);
char relativeFileName[1024];
FILE* f = 0;
bool found = false;
for (int i = 0; !f && i<numPrefixes; i++)
{
sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
f = fopen(relativeFileName, "rb");
if (f)
{
found = true;
break;
}
}
if (f)
{
fclose(f);
}
if (found)
{
ok = importer->loadFile(relativeFileName);
}
}
if (ok)
{
serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
}
return hasStatus;
}
bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("CMD_LOAD_BULLET");
@@ -8541,6 +8597,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_RESTORE_STATE:
{
hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_LOAD_BULLET:
{
hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);

1
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -80,6 +80,7 @@ protected:
bool processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSaveBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);

15
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -69,6 +69,13 @@ struct SdfArgs
struct FileArgs
{
char m_fileName[MAX_URDF_FILENAME_LENGTH];
int m_stateId;
};
enum EnumLoadStateArgsUpdateFlags
{
CMD_LOAD_STATE_HAS_STATEID=1,
CMD_LOAD_STATE_HAS_FILENAME=2,
};
enum EnumUrdfArgsUpdateFlags
@@ -922,6 +929,12 @@ struct b3ChangeTextureArgs
int m_height;
};
struct b3StateSerializationArguments
{
char m_fileName[MAX_URDF_FILENAME_LENGTH];
int m_stateId;
};
struct SharedMemoryCommand
{
int m_type;
@@ -976,6 +989,7 @@ struct SharedMemoryCommand
struct b3ChangeTextureArgs m_changeTextureArgs;
struct b3SearchPathfArgs m_searchPathArgs;
struct b3CustomCommand m_customCommandArgs;
struct b3StateSerializationArguments m_loadStateArguments;
};
};
@@ -1050,6 +1064,7 @@ struct SharedMemoryStatus
struct b3LoadTextureResultArgs m_loadTextureResultArguments;
struct b3CustomCommandResultArgs m_customCommandResultArgs;
struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
struct b3StateSerializationArguments m_saveStateResultArgs;
};
};

9
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -112,12 +112,13 @@ public:
{
int newargc = argc+2;
m_newargv = (char**)malloc(sizeof(void*)*newargc);
for (int i=0;i<argc;i++)
m_newargv[i] = argv[i];
char* t0 = (char*)"--unused";
m_newargv[0] = t0;
for (int i=0;i<argc;i++)
m_newargv[i+1] = argv[i];
char* t0 = (char*)"--logtostderr";
char* t1 = (char*)"--start_demo_name=Physics Server";
m_newargv[argc] = t0;
m_newargv[argc+1] = t1;
m_data = btCreateInProcessExampleBrowser(newargc,m_newargv, useInProcessMemory);
SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);

8
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -77,6 +77,8 @@ enum EnumSharedMemoryClientCommand
CMD_SET_ADDITIONAL_SEARCH_PATH,
CMD_CUSTOM_COMMAND,
CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS,
CMD_SAVE_STATE,
CMD_RESTORE_STATE,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -178,7 +180,11 @@ enum EnumSharedMemoryServerStatus
CMD_CUSTOM_COMMAND_COMPLETED,
CMD_CUSTOM_COMMAND_FAILED,
CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_SAVE_STATE_FAILED,
CMD_SAVE_STATE_COMPLETED,
CMD_RESTORE_STATE_FAILED,
CMD_RESTORE_STATE_COMPLETED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};

29
examples/pybullet/examples/saveRestoreState.py Normal file
View File

@@ -0,0 +1,29 @@
import pybullet as p
import math, time
p.connect(p.GUI, options="--width=1024 --height=768")
p.loadURDF("plane.urdf")
for i in range (10):
cube = p.loadURDF("cube_small.urdf",0,i*0.01,i*0.5)
p.setGravity(0,0,-10)
for i in range (500):
p.stepSimulation()
for i in range (10):
print("pos[",i,"]=",p.getBasePositionAndOrientation(i))
#saveState = 0
#if saveState:
# for i in range (500):
# p.stepSimulation()
# p.saveBullet("state.bullet")
#else:
# p.restoreState(fileName="state.bullet")
while (p.getConnectionInfo()["isConnected"]):
time.sleep(1)

122
examples/pybullet/pybullet.c
View File

@@ -15,6 +15,8 @@
#include <Python.h>
#endif
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
#ifdef B3_DUMP_PYTHON_VERSION
#define B3_VALUE_TO_STRING(x) #x
#define B3_VALUE(x) B3_VALUE_TO_STRING(x)
@@ -298,7 +300,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
int freeIndex = -1;
int method = eCONNECT_GUI;
int i;
char* options="";
char* options=0;
b3PhysicsClientHandle sm = 0;
@@ -352,12 +354,22 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
}
}
int argc = 0;
char** argv=0;
if (options)
{
argv = urdfStrSplit(options, " ");
argc = urdfStrArrayLen(argv);
for (int i = 0; i < argc; i++)
{
printf("argv[%d]=%s\n", i, argv[i]);
}
}
switch (method)
{
case eCONNECT_GUI:
{
int argc = 2;
char* argv[2] = {"unused",options};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
@@ -368,8 +380,6 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
}
case eCONNECT_GUI_SERVER:
{
int argc = 2;
char* argv[2] = {"unused",options};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(argc, argv);
@@ -419,6 +429,11 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
return NULL;
}
};
if (options)
{
urdfStrArrayFree(argv);
}
}
if (sm && b3CanSubmitCommand(sm))
@@ -588,7 +603,7 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args, PyObject* k
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bulletFileName", "physicsClientId", NULL};
static char* kwlist[] = {"bulletFileName", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName, &physicsClientId))
{
return NULL;
@@ -662,6 +677,93 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k
return Py_None;
}
static PyObject* pybullet_restoreState(PyObject* self, PyObject* args, PyObject* keywds)
{
const char* fileName = "";
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int stateId = -1;
b3SharedMemoryCommandHandle command;
PyObject* pylist = 0;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = { "stateId", "fileName", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|isi", kwlist, &stateId, &fileName, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3LoadStateCommandInit(sm);
if (stateId >= 0)
{
b3LoadStateSetStateId(command, stateId);
}
if (fileName)
{
b3LoadStateSetFileName(command, fileName);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_RESTORE_STATE_COMPLETED)
{
PyErr_SetString(SpamError, "Couldn't restore state.");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_saveState(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command;
b3PhysicsClientHandle sm = 0;
int stateId = -1;
int physicsClientId = 0;
static char* kwlist[] = { "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3SaveStateCommandInit(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SAVE_STATE_COMPLETED)
{
PyErr_SetString(SpamError, "Couldn't save state");
return NULL;
}
{
stateId = b3GetStatusGetStateId(statusHandle);
PyObject* pylist = 0;
pylist = PyTuple_New(1);
PyTuple_SetItem(pylist, 0, PyInt_FromLong(stateId));
return pylist;
}
}
static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds)
{
const char* mjcfFileName = "";
@@ -7761,11 +7863,17 @@ static PyMethodDef SpamMethods[] = {
"Load multibodies from an SDF file."},
{"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS,
"Restore the full state of the world from a .bullet file."},
"Load a world from a .bullet file."},
{"saveBullet", (PyCFunction)pybullet_saveBullet, METH_VARARGS | METH_KEYWORDS,
"Save the full state of the world to a .bullet file."},
{ "restoreState", (PyCFunction)pybullet_restoreState, METH_VARARGS | METH_KEYWORDS,
"Restore the full state of an existing world." },
// { "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
// "Save the full state of the world to memory." },
{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
"Load multibodies from an MJCF file."},