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

Merge pull request #1487 from erwincoumans/master

Browse Source 
expose link index in segmentation mask (getCameraImage) when a flag is set. preparation for restoreState (not implemented yet), allow multiple options in connect call.
This commit is contained in:
erwincoumans
2017-12-29 10:55:15 -08:00
committed by GitHub
parent 60d2af8ed2 29cfac096b
commit 28803dc56f
26 changed files with 695 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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;i<bulletFile2->m_multiBodies.size();i++)
bool result = false;
btAlignedObjectArray<btQuaternion> scratchQ;
btAlignedObjectArray<btVector3> scratchM;
if (m_importerFlags&eRESTORE_EXISTING_OBJECTS)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
//check if the snapshot is valid for the existing world
//equal number of objects, # links etc
if (bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies())
{
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*) bulletFile2->m_multiBodies[i];
bool isFixedBase = mbd->m_baseMass==0;
bool canSleep = false;
btVector3 baseInertia;
baseInertia.deSerializeDouble(mbd->m_baseInertia);
btMultiBody* mb = new btMultiBody(mbd->m_numLinks,mbd->m_baseMass,baseInertia,isFixedBase,canSleep);
mb->setHasSelfCollision(false);
result = false;
return result;
}
result = true;
m_data->m_mbMap.insert(mbd,mb);
for (int i=0;i<mbd->m_numLinks;i++)
//convert all multibodies
for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btVector3 localInertiaDiagonal;
localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
btQuaternion parentRotToThis;
parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
btVector3 parentComToThisPivotOffset;
parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
btVector3 thisPivotToThisComOffset;
thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
bool isFixedBase = mbd->m_baseMass == 0;
bool canSleep = false;
btVector3 baseInertia;
baseInertia.deSerializeDouble(mbd->m_baseInertia);
switch (mbd->m_links[i].m_jointType)
btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
mb->setBaseWorldTransform(tr);
for (int i = 0; i < mbd->m_numLinks; i++)
{
}
btVector3 linvel = mb->getBaseVel();
btVector3 angvel = mb->getBaseOmega();
mb->forwardKinematics(scratchQ, scratchM);
mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
}
}
}
else
{
result = btBulletWorldImporter::convertAllObjects(bulletFile2);
//convert all multibodies
for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
bool isFixedBase = mbd->m_baseMass == 0;
bool canSleep = false;
btVector3 baseInertia;
baseInertia.deSerializeDouble(mbd->m_baseInertia);
btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep);
mb->setHasSelfCollision(false);
btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
mb->setBaseWorldTransform(tr);
m_data->m_mbMap.insert(mbd, mb);
for (int i = 0; i < mbd->m_numLinks; i++)
{
btVector3 localInertiaDiagonal;
localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
btQuaternion parentRotToThis;
parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
btVector3 parentComToThisPivotOffset;
parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
btVector3 thisPivotToThisComOffset;
thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
switch (mbd->m_links[i].m_jointType)
{
case btMultibodyLink::eFixed:
{
mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
parentRotToThis, parentComToThisPivotOffset,thisPivotToThisComOffset);
parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
//search for the collider
//mbd->m_links[i].m_linkCollider
break;
@@ -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;i<bulletFile2->m_multiBodyLinkColliders.size();i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*) bulletFile2->m_multiBodyLinkColliders[i];
btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
//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;i<m_data->m_mbMap.size();i++)
{
btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
if (ptr)
//convert all multibody link colliders
for (int i = 0; i < bulletFile2->m_multiBodyLinkColliders.size(); i++)
{
btMultiBody* mb = *ptr;
mb->finalizeMultiDof();
m_data->m_mbDynamicsWorld->addMultiBody(mb);
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*)bulletFile2->m_multiBodyLinkColliders[i];
btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
if (ptr)
{
btMultiBody* multiBody = *ptr;
btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
if (shapePtr && *shapePtr)
{
btTransform startTransform;
mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
if (shape)
{
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
col->setCollisionShape(shape);
//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
//m_bodyMap.insert(colObjData,body);
if (mblcd->m_link == -1)
{
col->setWorldTransform(multiBody->getBaseWorldTransform());
multiBody->setBaseCollider(col);
}
else
{
col->setWorldTransform(multiBody->getLink(mblcd->m_link).m_cachedWorldTransform);
multiBody->getLink(mblcd->m_link).m_collider = col;
}
int mbLinkIndex = mblcd->m_link;
bool isDynamic = (mbLinkIndex < 0 && multiBody->hasFixedBase()) ? false : true;
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
#if 0
int colGroup = 0, colMask = 0;
int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
if (collisionFlags & URDF_HAS_COLLISION_GROUP)
{
collisionFilterGroup = colGroup;
}
if (collisionFlags & URDF_HAS_COLLISION_MASK)
{
collisionFilterMask = colMask;
}
#endif
m_data->m_mbDynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
}
}
else
{
printf("error: no shape found\n");
}
#if 0
//base and fixed? -> static, otherwise flag as dynamic
world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
#endif
}
}
}
for (int i = 0; i < m_data->m_mbMap.size(); i++)
{
btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
if (ptr)
{
btMultiBody* mb = *ptr;
mb->finalizeMultiDof();
m_data->m_mbDynamicsWorld->addMultiBody(mb);
}
}
}
return result;

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

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

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

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

4
data/cube_small.urdf
View File

@@ -2,8 +2,8 @@
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<inertia_scaling value="3.0"/>
<friction_anchor/>
<lateral_friction value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>

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

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

2
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@@ -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];

99
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

12
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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);

7
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -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

13
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -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);

13
examples/SharedMemory/PhysicsDirect.cpp
View File

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

69
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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 && i<numPrefixes; i++)
{
sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
f = fopen(relativeFileName, "rb");
if (f)
{
found = true;
break;
}
}
if (f)
{
fclose(f);
}
if (found)
{
ok = importer->loadFile(relativeFileName);
}
}
if (ok)
{
serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
}
return hasStatus;
}
bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("CMD_LOAD_BULLET");
@@ -8541,6 +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);

1
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

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

6
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -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(),

18
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -69,6 +69,13 @@ struct SdfArgs
struct FileArgs
{
char m_fileName[MAX_URDF_FILENAME_LENGTH];
int m_stateId;
};
enum EnumLoadStateArgsUpdateFlags
{
CMD_LOAD_STATE_HAS_STATEID=1,
CMD_LOAD_STATE_HAS_FILENAME=2,
};
enum EnumUrdfArgsUpdateFlags
@@ -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;
};
};

9
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

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

12
examples/SharedMemory/SharedMemoryPublic.h
View File

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

23
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -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)
{
@@ -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)

1
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@@ -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);

9
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -199,7 +199,7 @@ m_objectIndex(-1)
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* 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<clippedTriangles.size();t++)
{
triangleClipped(clippedTriangles[t], shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
triangleClipped(clippedTriangles[t], shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex+((renderData.m_linkIndex + 1) << 24));
}
}
else
{
triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex+((renderData.m_linkIndex + 1) << 24));
}
}
}

3
examples/TinyRenderer/TinyRenderer.h
View File

@@ -39,7 +39,7 @@ struct TinyRenderObjectData
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* 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;
};

8
examples/TinyRenderer/our_gl.cpp
View File

@@ -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);
}

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

@@ -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)

42
examples/pybullet/examples/segmask_linkindex.py Normal file
View File

@@ -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()

138
examples/pybullet/pybullet.c
View File

@@ -15,6 +15,8 @@
#include <Python.h>
#endif
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
#ifdef B3_DUMP_PYTHON_VERSION
#define B3_VALUE_TO_STRING(x) #x
#define B3_VALUE(x) B3_VALUE_TO_STRING(x)
@@ -298,7 +300,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
int freeIndex = -1;
int method = eCONNECT_GUI;
int i;
char* options="";
char* options=0;
b3PhysicsClientHandle sm = 0;
@@ -352,12 +354,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);

2
setup.py
View File

@@ -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',