From 799d030874dcf940d8c4f523da2b968dfc2217f9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 22 Dec 2017 14:45:36 -0800 Subject: [PATCH 1/7] restore multibody world transform and joint angles/velocities from a .bullet file. --- .../btMultiBodyWorldImporter.cpp | 28 ++++++++++++++++++- data/cube_small.urdf | 4 +-- .../PhysicsClientSharedMemory.cpp | 4 +++ 3 files changed, 33 insertions(+), 3 deletions(-) diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index 9b0dca0c0..d80761af6 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp @@ -52,6 +52,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF 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;im_numLinks;i++) @@ -84,6 +86,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF 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->setJointPos(i, mbd->m_links[i].m_jointPos[0]); + mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); break; } case btMultibodyLink::eRevolute: @@ -93,6 +97,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF 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->setJointPos(i, mbd->m_links[i].m_jointPos[0]); + mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]); break; } case btMultibodyLink::eSpherical: @@ -101,6 +107,9 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF 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->setJointPosMultiDof(i, mbd->m_links[i].m_jointPos); + mb->setJointVelMultiDof(i, mbd->m_links[i].m_jointVel); + break; } case btMultibodyLink::ePlanar: @@ -117,6 +126,22 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF } } + //forward kinematics, so that the link world transforms are valid, for collision detection + btAlignedObjectArray 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++) { @@ -142,16 +167,17 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF { 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; 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/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 463082980..593838793 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1236,6 +1236,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { { break; } + case CMD_BULLET_SAVING_COMPLETED: + { + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); From ddabc6b70cc52f9461d37fb5ea1e7f450863449a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 23 Dec 2017 11:02:22 -0800 Subject: [PATCH 2/7] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index cf5582b73..bc6d7aca2 100644 --- a/setup.py +++ b/setup.py @@ -442,7 +442,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', From 97547eda0db50a34bf6ceb265a3089fb034df180 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 28 Dec 2017 10:05:51 -0800 Subject: [PATCH 3/7] preliminary work towards saveState/restoreState and saveRestoreState.py example (not implemented yet) allow multiple options in connect, for example: p.connect(p.GUI, options="--width=640, --height=480") --- .../btMultiBodyWorldImporter.cpp | 299 ++++++++++-------- .../BulletWorldImporter/btWorldImporter.cpp | 3 +- .../BulletWorldImporter/btWorldImporter.h | 17 + .../ImportURDFDemo/urdfStringSplit.h | 12 +- examples/SharedMemory/PhysicsClientC_API.cpp | 85 +++++ examples/SharedMemory/PhysicsClientC_API.h | 9 + .../PhysicsClientSharedMemory.cpp | 9 + examples/SharedMemory/PhysicsDirect.cpp | 13 + .../PhysicsServerCommandProcessor.cpp | 62 ++++ .../PhysicsServerCommandProcessor.h | 1 + examples/SharedMemory/SharedMemoryCommands.h | 15 + .../SharedMemoryInProcessPhysicsC_API.cpp | 9 +- examples/SharedMemory/SharedMemoryPublic.h | 8 +- .../pybullet/examples/saveRestoreState.py | 29 ++ examples/pybullet/pybullet.c | 124 +++++++- 15 files changed, 552 insertions(+), 143 deletions(-) create mode 100644 examples/pybullet/examples/saveRestoreState.py 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."}, From afd1066c50991a626b81f91edd5b63c8dd5fd1c4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 28 Dec 2017 10:18:35 -0800 Subject: [PATCH 4/7] pybullet.getCameraImage: preparation to expose link index in segmentation mask buffer --- examples/RenderingExamples/TinyRendererSetup.cpp | 2 +- examples/SharedMemory/PhysicsClientExample.cpp | 7 +++++-- examples/SharedMemory/PhysicsServerExample.cpp | 6 +++++- .../TinyRendererVisualShapeConverter.cpp | 2 +- examples/TinyRenderer/TinyRenderer.cpp | 9 +++++---- examples/TinyRenderer/TinyRenderer.h | 3 ++- examples/TinyRenderer/our_gl.cpp | 15 +++++++++++---- 7 files changed, 30 insertions(+), 14 deletions(-) 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/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 86aaa1a8a..08adb66c6 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&(0x1e24-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/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 518f1b657..8d13bf105 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&(0x1e24-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/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index d44defa34..c687732da 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -629,7 +629,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; diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index e722f4a2c..dbf6522e8 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..c13e345a8 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,14 @@ 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; + if (objectAndLinkIndex) + { + segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex; + } else + { + segmentationMaskBuffer[P.x+P.y*image.get_width()] = 0; + + } } image.set(P.x, P.y, color); } From 5517cbc4e016798289f8ebf835a650706a6490ee Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 28 Dec 2017 12:37:07 -0800 Subject: [PATCH 5/7] add segmask_linkindex.py example using p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX) to extract object unique id and link index from segmentation mask in getCameraImage --- examples/SharedMemory/PhysicsClientC_API.cpp | 14 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 3 ++ .../SharedMemory/PhysicsClientExample.cpp | 2 +- .../PhysicsServerCommandProcessor.cpp | 7 +++- .../SharedMemory/PhysicsServerExample.cpp | 2 +- examples/SharedMemory/SharedMemoryCommands.h | 3 ++ examples/SharedMemory/SharedMemoryPublic.h | 4 ++ .../TinyRendererVisualShapeConverter.cpp | 21 +++++++++- .../TinyRendererVisualShapeConverter.h | 1 + examples/TinyRenderer/TinyRenderer.cpp | 4 +- examples/TinyRenderer/our_gl.cpp | 9 +--- .../pybullet/examples/saveRestoreState.py | 27 ++++++------ .../pybullet/examples/segmask_linkindex.py | 42 +++++++++++++++++++ examples/pybullet/pybullet.c | 13 ++++-- 14 files changed, 120 insertions(+), 32 deletions(-) create mode 100644 examples/pybullet/examples/segmask_linkindex.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 5efa52f08..cacd375a0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2940,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 7aaabba87..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 diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 08adb66c6..355f5b9c5 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -940,7 +940,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) btVector4(32,255,255,255)}; if (segmentationMask>=0) { - int obIndex = segmentationMask&(0x1e24-1); + int obIndex = segmentationMask&((1<<24)-1); int linkIndex = (segmentationMask>>24)-1; btVector4 rgb = palette[(obIndex+linkIndex)&3]; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f9a6f20a5..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; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 8d13bf105..2a53c58e5 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -2169,7 +2169,7 @@ void PhysicsServerExample::updateGraphics() btVector4(32,255,255,255)}; if (segmentationMask>=0) { - int obIndex = segmentationMask&(0x1e24-1); + int obIndex = segmentationMask&((1<<24)-1); int linkIndex = (segmentationMask>>24)-1; btVector4 rgb = palette[(obIndex+linkIndex)&3]; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 3f6864ba8..e953104a7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -238,6 +238,7 @@ struct RequestPixelDataArgs float m_lightDiffuseCoeff; float m_lightSpecularCoeff; int m_hasShadow; + int m_flags; }; enum EnumRequestPixelDataUpdateFlags @@ -251,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 }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index bb9bd8be1..b152a70c3 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -584,6 +584,10 @@ enum EnumRenderer //ER_FIRE_RAYS=(1<<18), }; +enum EnumRendererAuxFlags +{ + ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX=1, +}; ///flags to pick the IK solver and other options enum EnumCalculateInverseKinematicsFlags { diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index c687732da..d0a9c321a 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -82,6 +82,7 @@ struct TinyRendererVisualShapeConverterInternalData float m_lightSpecularCoeff; bool m_hasLightSpecularCoeff; bool m_hasShadow; + int m_flags; SimpleCamera m_camera; @@ -102,7 +103,8 @@ struct TinyRendererVisualShapeConverterInternalData m_hasLightDiffuseCoeff(false), m_lightSpecularCoeff(0.05), m_hasLightSpecularCoeff(false), - m_hasShadow(false) + m_hasShadow(false), + m_flags(0) { m_depthBuffer.resize(m_swWidth*m_swHeight); m_shadowBuffer.resize(m_swWidth*m_swHeight); @@ -154,6 +156,11 @@ void TinyRendererVisualShapeConverter::setShadow(bool hasShadow) { m_data->m_hasShadow = hasShadow; } +void TinyRendererVisualShapeConverter::setFlags(int flags) +{ + m_data->m_flags = flags; +} + void TinyRendererVisualShapeConverter::setLightAmbientCoeff(float ambientCoeff) { @@ -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 dbf6522e8..824791d55 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -562,12 +562,12 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) { for (int t=0;t &clipc, IShader &shader, TGAImage &image, float *zb zbuffer[P.x+P.y*image.get_width()] = frag_depth; if (segmentationMaskBuffer) { - if (objectAndLinkIndex) - { - segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex; - } else - { - segmentationMaskBuffer[P.x+P.y*image.get_width()] = 0; - - } + 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 index 0361bedba..cffc4916b 100644 --- a/examples/pybullet/examples/saveRestoreState.py +++ b/examples/pybullet/examples/saveRestoreState.py @@ -9,21 +9,22 @@ for i in range (10): p.setGravity(0,0,-10) -for i in range (500): - p.stepSimulation() +#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)) - -#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) \ 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 d2cfe7636..8e5370594 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6046,16 +6046,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; } @@ -6107,6 +6107,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 @@ -8365,7 +8369,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); From 22e23d22d11d09b3c52fe0b0797098a610a8a17d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 28 Dec 2017 12:55:47 -0800 Subject: [PATCH 6/7] double/single precision compile fix --- .../BulletWorldImporter/btMultiBodyWorldImporter.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index 383334db8..1704a1e39 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp @@ -149,8 +149,10 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF 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->setJointPosMultiDof(i, mbd->m_links[i].m_jointPos); - mb->setJointVelMultiDof(i, mbd->m_links[i].m_jointVel); + 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; } From 29cfac096b99387a2fb61f0b23be994f6cdd2902 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 28 Dec 2017 12:57:49 -0800 Subject: [PATCH 7/7] compile fix in pybullet.c --- examples/pybullet/pybullet.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8e5370594..47aeee8a1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -358,9 +358,10 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P char** argv=0; if (options) { + int i; argv = urdfStrSplit(options, " "); argc = urdfStrArrayLen(argv); - for (int i = 0; i < argc; i++) + for (i = 0; i < argc; i++) { printf("argv[%d]=%s\n", i, argv[i]); }