diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index 9b0dca0c0..1704a1e39 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp @@ -36,43 +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); + 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; @@ -82,8 +126,10 @@ 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; } case btMultibodyLink::eRevolute: @@ -91,16 +137,23 @@ 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; } case btMultibodyLink::eSpherical: { 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); + btScalar jointPos[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] }; + btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[2] }; + mb->setJointPosMultiDof(i, jointPos); + mb->setJointVelMultiDof(i, jointVel); + break; } case btMultibodyLink::ePlanar: @@ -112,92 +165,110 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF { btAssert(0); } + } } } } - } - - //convert all multibody link colliders - for (int i=0;im_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]; + //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**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) - { - multiBody->setBaseCollider(col); - } else - { - 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/data/cube_small.urdf b/data/cube_small.urdf index 299ded801..b93c92958 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -2,8 +2,8 @@ - - + + 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/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 64fa77f14..61ead53d5 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -195,7 +195,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) m_internalData->m_depthBuffer, &m_internalData->m_shadowBuffer, &m_internalData->m_segmentationMaskBuffer, - m_internalData->m_renderObjects.size()); + m_internalData->m_renderObjects.size(), -1); meshData.m_gfxShape->m_scaling[0] = scaling[0]; meshData.m_gfxShape->m_scaling[1] = scaling[1]; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 09f6a9a2d..cacd375a0 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; @@ -2855,6 +2940,20 @@ B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandl } } +B3_SHARED_API void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + if(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA) + { + command->m_requestPixelDataArguments.m_flags = flags; + command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_FLAGS; + } +} + + + B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 0e6584ee1..d1d76815f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -209,6 +209,9 @@ B3_SHARED_API void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryComman B3_SHARED_API void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff); B3_SHARED_API void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow); B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer); +B3_SHARED_API void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); + + B3_SHARED_API void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); ///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices @@ -313,7 +316,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/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 86aaa1a8a..355f5b9c5 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -940,8 +940,11 @@ void PhysicsClientExample::stepSimulation(float deltaTime) btVector4(32,255,255,255)}; if (segmentationMask>=0) { - btVector4 rgb = palette[segmentationMask&3]; - m_canvas->setPixel(m_canvasSegMaskIndex,i,j, + int obIndex = segmentationMask&((1<<24)-1); + int linkIndex = (segmentationMask>>24)-1; + + btVector4 rgb = palette[(obIndex+linkIndex)&3]; + m_canvas->setPixel(m_canvasSegMaskIndex,i,j, rgb.x(), rgb.y(), rgb.z(), 255); //alpha set to 255 diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 463082980..9ca17b8e8 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1236,6 +1236,19 @@ 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; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); 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..b3d24b3e6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3017,7 +3017,12 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, clientCmd.m_requestPixelDataArguments.m_pixelHeight); } - + int flags = 0; + if (clientCmd.m_updateFlags&REQUEST_PIXEL_ARGS_HAS_FLAGS) + { + flags = clientCmd.m_requestPixelDataArguments.m_flags; + } + m_data->m_visualConverter.setFlags(flags); int numTotalPixels = width*height; int numRemainingPixels = numTotalPixels - startPixelIndex; @@ -8121,6 +8126,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 +8602,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/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 518f1b657..2a53c58e5 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -2169,7 +2169,11 @@ void PhysicsServerExample::updateGraphics() btVector4(32,255,255,255)}; if (segmentationMask>=0) { - btVector4 rgb = palette[segmentationMask&3]; + int obIndex = segmentationMask&((1<<24)-1); + int linkIndex = (segmentationMask>>24)-1; + + btVector4 rgb = palette[(obIndex+linkIndex)&3]; + m_canvas->setPixel(m_canvasSegMaskIndex,i,j, rgb.x(), rgb.y(), diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 60cc18c38..e953104a7 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 @@ -231,6 +238,7 @@ struct RequestPixelDataArgs float m_lightDiffuseCoeff; float m_lightSpecularCoeff; int m_hasShadow; + int m_flags; }; enum EnumRequestPixelDataUpdateFlags @@ -244,6 +252,8 @@ enum EnumRequestPixelDataUpdateFlags REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF=64, REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF=128, REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF=256, + REQUEST_PIXEL_ARGS_HAS_FLAGS = 512, + //don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h }; @@ -922,6 +932,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 +992,7 @@ struct SharedMemoryCommand struct b3ChangeTextureArgs m_changeTextureArgs; struct b3SearchPathfArgs m_searchPathArgs; struct b3CustomCommand m_customCommandArgs; + struct b3StateSerializationArguments m_loadStateArguments; }; }; @@ -1050,6 +1067,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;im_hasShadow = hasShadow; } +void TinyRendererVisualShapeConverter::setFlags(int flags) +{ + m_data->m_flags = flags; +} + void TinyRendererVisualShapeConverter::setLightAmbientCoeff(float ambientCoeff) { @@ -629,7 +636,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( if (vertices.size() && indices.size()) { - TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId); + TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex); unsigned char* textureImage1=0; int textureWidth=0; int textureHeight=0; @@ -1043,7 +1050,17 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels } if (segmentationMaskBuffer) { - segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex]; + int segMask = m_data->m_segmentationMaskBuffer[i + startPixelIndex]; + if ((m_data->m_flags & ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)==0) + { + //if we don't explicitly request link index, clear it out + //object index are the lower 24bits + if (segMask >= 0) + { + segMask &= ((1 << 24) - 1); + } + } + segmentationMaskBuffer[i] = segMask; } if (pixelsRGBA) diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 97bbb0a36..4c5abbfbb 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -43,6 +43,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void setLightDiffuseCoeff(float diffuseCoeff); void setLightSpecularCoeff(float specularCoeff); void setShadow(bool hasShadow); + void setFlags(int flags); void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index e722f4a2c..824791d55 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -199,7 +199,7 @@ m_objectIndex(-1) } -TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray* shadowBuffer, b3AlignedObjectArray* segmentationMaskBuffer, int objectIndex) +TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray* shadowBuffer, b3AlignedObjectArray* segmentationMaskBuffer, int objectIndex, int linkIndex) :m_model(0), m_rgbColorBuffer(rgbColorBuffer), m_depthBuffer(depthBuffer), @@ -207,7 +207,8 @@ m_shadowBuffer(shadowBuffer), m_segmentationMaskBufferPtr(segmentationMaskBuffer), m_userData(0), m_userIndex(-1), -m_objectIndex(objectIndex) +m_objectIndex(objectIndex), +m_linkIndex(linkIndex) { Vec3f eye(1,1,3); Vec3f center(0,0,0); @@ -561,12 +562,12 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) { for (int t=0;t&depthBuffer); TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer,b3AlignedObjectArray* segmentationMaskBuffer,int objectIndex); TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer,b3AlignedObjectArray* shadowBuffer); - TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer,b3AlignedObjectArray* shadowBuffer, b3AlignedObjectArray* segmentationMaskBuffer,int objectIndex); + TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer,b3AlignedObjectArray* shadowBuffer, b3AlignedObjectArray* segmentationMaskBuffer,int objectIndex, int linkIndex); virtual ~TinyRenderObjectData(); void loadModel(const char* fileName); @@ -52,6 +52,7 @@ struct TinyRenderObjectData void* m_userData; int m_userIndex; int m_objectIndex; + int m_linkIndex; }; diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp index 8ea08f00b..9f6e46e45 100644 --- a/examples/TinyRenderer/our_gl.cpp +++ b/examples/TinyRenderer/our_gl.cpp @@ -75,7 +75,7 @@ void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &s triangleClipped(clipc, orgClipc,shader,image,zbuffer,0,viewPortMatrix,0); } -void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) +void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectAndLinkIndex) { mat<3,4,float> screenSpacePts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points @@ -135,7 +135,7 @@ void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &s zbuffer[P.x+P.y*image.get_width()] = frag_depth; if (segmentationMaskBuffer) { - segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex; + segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex; } image.set(P.x, P.y, color); } @@ -149,7 +149,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0); } -void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) { +void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectAndLinkIndex) { mat<3,4,float> pts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points @@ -189,7 +189,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb zbuffer[P.x+P.y*image.get_width()] = frag_depth; if (segmentationMaskBuffer) { - segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex; + segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex; } image.set(P.x, P.y, color); } diff --git a/examples/pybullet/examples/saveRestoreState.py b/examples/pybullet/examples/saveRestoreState.py new file mode 100644 index 000000000..cffc4916b --- /dev/null +++ b/examples/pybullet/examples/saveRestoreState.py @@ -0,0 +1,30 @@ +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() + + + +saveState = 1 + +if saveState: + for i in range (500): + p.stepSimulation() + p.saveBullet("state.bullet") +else: + p.restoreState(fileName="state.bullet") + +for i in range (10): + print("pos[",i,"]=",p.getBasePositionAndOrientation(i)) + +while (p.getConnectionInfo()["isConnected"]): + time.sleep(1) \ No newline at end of file diff --git a/examples/pybullet/examples/segmask_linkindex.py b/examples/pybullet/examples/segmask_linkindex.py new file mode 100644 index 000000000..0c6ad2ff7 --- /dev/null +++ b/examples/pybullet/examples/segmask_linkindex.py @@ -0,0 +1,42 @@ +import pybullet as p +p.connect(p.GUI) +r2d2 = p.loadURDF("r2d2.urdf",[0,0,1]) +for l in range (p.getNumJoints(r2d2)): + print(p.getJointInfo(r2d2,l)) + +p.loadURDF("r2d2.urdf",[2,0,1]) +p.loadURDF("r2d2.urdf",[4,0,1]) + +p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX) +segLinkIndex=1 +verbose = 0 + +while (1): + keys = p.getKeyboardEvents() + #for k in keys: + # print("key=",k,"state=", keys[k]) + if ord('1') in keys: + state = keys[ord('1')] + if (state & p.KEY_WAS_RELEASED): + verbose = 1-verbose + if ord('s') in keys: + state = keys[ord('s')] + if (state & p.KEY_WAS_RELEASED): + segLinkIndex = 1-segLinkIndex + #print("segLinkIndex=",segLinkIndex) + flags = 0 + if (segLinkIndex): + flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX + + img = p.getCameraImage(320,200,flags=flags) + #print(img[0],img[1]) + seg=img[4] + if (verbose): + for pixel in seg: + if (pixel>=0): + obUid = pixel & ((1<<24)-1) + linkIndex = (pixel >> 24)-1 + print("obUid=",obUid,"linkIndex=",linkIndex) + + + p.stepSimulation() \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 90bef2b32..47aeee8a1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -15,6 +15,8 @@ #include #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,23 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P } } + int argc = 0; + char** argv=0; + if (options) + { + int i; + argv = urdfStrSplit(options, " "); + argc = urdfStrArrayLen(argv); + for (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 +381,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 +430,11 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P return NULL; } }; + + if (options) + { + urdfStrArrayFree(argv); + } } if (sm && b3CanSubmitCommand(sm)) @@ -588,7 +604,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 +678,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 = ""; @@ -5944,16 +6047,16 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec float lightAmbientCoeff = -1; float lightDiffuseCoeff = -1; float lightSpecularCoeff = -1; - + int flags = -1; int renderer = -1; // inialize cmd b3SharedMemoryCommandHandle command; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; // set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow - static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "physicsClientId", NULL}; + static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "flags", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffiii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &flags, &physicsClientId)) { return NULL; } @@ -6005,6 +6108,10 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff); } + if (flags >= 0) + { + b3RequestCameraImageSetFlags(command, flags); + } if (renderer>=0) { b3RequestCameraImageSelectRenderer(command, renderer);//renderer could be ER_BULLET_HARDWARE_OPENGL @@ -7761,11 +7868,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."}, @@ -8257,7 +8370,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER); PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL); - + PyModule_AddIntConstant(m, "ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX", ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX); + PyModule_AddIntConstant(m, "IK_DLS", IK_DLS); PyModule_AddIntConstant(m, "IK_SDLS", IK_SDLS); PyModule_AddIntConstant(m, "IK_HAS_TARGET_POSITION", IK_HAS_TARGET_POSITION); diff --git a/setup.py b/setup.py index 9123f2ecd..f1508d75b 100644 --- a/setup.py +++ b/setup.py @@ -443,7 +443,7 @@ print("-----") setup( name = 'pybullet', - version='1.7.5', + version='1.7.6', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3',