diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index d80761af6..383334db8 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp @@ -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;im_multiBodies.size();i++) + bool result = false; + btAlignedObjectArray scratchQ; + btAlignedObjectArray 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;im_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 scratchQ; - btAlignedObjectArray scratchM; - for (int i = 0; im_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;im_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;im_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; diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp index d800f3a5e..080b19aef 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp @@ -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) { } diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.h b/Extras/Serialize/BulletWorldImporter/btWorldImporter.h index ba1c38d12..97c621fb5 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.h +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.h @@ -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 m_allocatedCollisionShapes; btAlignedObjectArray 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); diff --git a/examples/Importers/ImportURDFDemo/urdfStringSplit.h b/examples/Importers/ImportURDFDemo/urdfStringSplit.h index dd7af9158..be6b8f5c3 100644 --- a/examples/Importers/ImportURDFDemo/urdfStringSplit.h +++ b/examples/Importers/ImportURDFDemo/urdfStringSplit.h @@ -2,14 +2,20 @@ #ifndef STRING_SPLIT_H #define STRING_SPLIT_H +#ifdef __cplusplus + #include #include #include "LinearMath/btAlignedObjectArray.h" - void urdfStringSplit( btAlignedObjectArray&pieces, const std::string& vector_str, const btAlignedObjectArray& separators); void urdfIsAnyOf(const char* seps, btAlignedObjectArray& 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 diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 09f6a9a2d..5efa52f08 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 0e6584ee1..7aaabba87 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 593838793..9ca17b8e8 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -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; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 6dbac65db..9b6a2b4f3 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -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"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 78de50e10..f9a6f20a5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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 && iloadFile(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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 6177d78fe..fb26b52fb 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -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); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 60cc18c38..3f6864ba8 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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; }; }; diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 3c13898ba..fa1a31201 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -112,12 +112,13 @@ public: { int newargc = argc+2; m_newargv = (char**)malloc(sizeof(void*)*newargc); - for (int i=0;i #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,9 +380,7 @@ 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); #else @@ -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."},