From b23cb1dd2c838e815402fc0fdaf77325e1834a1e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 3 Jun 2017 10:57:56 -0700 Subject: [PATCH] pybullet.createCollisionShape, createVisualShape, createMultiBody, programmatic creation using ProgrammaticUrdfInterface (still preliminary, not ready for commit yet, see examples\pybullet\examples\createSphereMultiBodies.py) --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 8 +- .../ImportURDFDemo/BulletUrdfImporter.h | 1 + .../ImportURDFDemo/URDFImporterInterface.h | 3 +- .../Importers/ImportURDFDemo/UrdfParser.h | 3 +- .../MultiBody/InvertedPendulumPDControl.cpp | 2 +- examples/OpenGLWindow/SimpleOpenGL2App.cpp | 7 +- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 8 +- examples/SharedMemory/PhysicsClientC_API.cpp | 199 ++++++++ examples/SharedMemory/PhysicsClientC_API.h | 22 + .../PhysicsClientSharedMemory.cpp | 24 + examples/SharedMemory/PhysicsDirect.cpp | 28 ++ .../PhysicsServerCommandProcessor.cpp | 435 +++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 95 ++++ examples/SharedMemory/SharedMemoryPublic.h | 24 + .../examples/createSphereMultiBodies.py | 30 ++ examples/pybullet/pybullet.c | 165 +++++++ 16 files changed, 1032 insertions(+), 22 deletions(-) create mode 100644 examples/pybullet/examples/createSphereMultiBodies.py diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 72aaecfce..d243c1bc9 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -294,7 +294,6 @@ std::string BulletURDFImporter::getJointName(int linkIndex) const void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const { - //todo(erwincoumans) //the link->m_inertia is NOT necessarily aligned with the inertial frame //so an additional transform might need to be computed UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); @@ -414,6 +413,13 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi } +void BulletURDFImporter::setRootTransformInWorld(const btTransform& rootTransformInWorld) +{ + m_data->m_urdfParser.getModel().m_rootTransformInWorld = rootTransformInWorld ; +} + + + bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const { rootTransformInWorld = m_data->m_urdfParser.getModel().m_rootTransformInWorld; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 5ada57d25..0723712dd 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -55,6 +55,7 @@ public: virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const; virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; + virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld); virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 588930184..d6853e3b6 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -64,7 +64,8 @@ public: }; virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0; - + virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld){} + ///quick hack: need to rethink the API/dependencies of this virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;} diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 1a74cbba2..817536916 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -282,12 +282,11 @@ public: int getNumModels() const { //user should have loaded an SDF when calling this method - btAssert(m_parseSDF); if (m_parseSDF) { return m_sdfModels.size(); } - return 0; + return 1; } void activateModel(int modelIndex); diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index 8031e773d..d380929c4 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -244,7 +244,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2); - btVector3 color(0.0,0.0,0.5); + btVector4 color(0.0,0.0,0.5,1); guiHelper->createCollisionObjectGraphicsObject(col,color); // col->setFriction(friction); diff --git a/examples/OpenGLWindow/SimpleOpenGL2App.cpp b/examples/OpenGLWindow/SimpleOpenGL2App.cpp index 198a8bf67..43b8058f2 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2App.cpp @@ -481,7 +481,8 @@ void SimpleOpenGL2App::drawText3D( const char* txt, float worldPosX, float world void SimpleOpenGL2App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4]) { b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5); - cubeExtents[m_data->m_upAxis] = 0; + double halfHeight=0.1; + cubeExtents[m_data->m_upAxis] = halfHeight; int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]); b3Quaternion orn(0,0,0,1); @@ -501,10 +502,10 @@ void SimpleOpenGL2App::registerGrid(int cells_x, int cells_z, float color0[4], f } if (this->m_data->m_upAxis==1) { - center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f); + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, -halfHeight, (j + 0.5f) - cells_z * 0.5f); } else { - center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f ); + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,-halfHeight ); } m_renderer->registerGraphicsInstance(cubeId,center,orn,color,scaling); } diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index e05056e81..e4475f535 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -704,9 +704,9 @@ int SimpleOpenGL3App::registerCubeShape(float halfExtentsX,float halfExtentsY, f void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4]) { b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5); - cubeExtents[m_data->m_upAxis] = 0; + double halfHeight=0.1; + cubeExtents[m_data->m_upAxis] = halfHeight; int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]); - b3Quaternion orn(0,0,0,1); b3Vector3 center=b3MakeVector3(0,0,0,1); b3Vector3 scaling=b3MakeVector3(1,1,1,1); @@ -724,10 +724,10 @@ void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], f } if (this->m_data->m_upAxis==1) { - center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f); + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, -halfHeight, (j + 0.5f) - cells_z * 0.5f); } else { - center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f ); + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,-halfHeight ); } m_instancingRenderer->registerGraphicsInstance(cubeId,center,orn,color,scaling); } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 33066175c..c8b29a115 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -691,6 +691,205 @@ int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle return 0; } +b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + if (cl) + { + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_CREATE_COLLISION_SHAPE; + command->m_updateFlags =0; + command->m_createCollisionShapeArgs.m_numCollisionShapes = 0; + return (b3SharedMemoryCommandHandle) command; + } + return 0; +} +void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + } + } +} + +void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[2] = halfExtents[2]; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + } + } +} + + +void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + if (shapeIndexm_createCollisionShapeArgs.m_numCollisionShapes) + { + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 1; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[0] = childPosition[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[1] = childPosition[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[2] = childPosition[2]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[0] = childOrientation[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[1] = childOrientation[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[2] = childOrientation[2]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[3] = childOrientation[3]; + } + } +} + + +int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED); + if (status && status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED) + { + return status->m_createCollisionShapeResultArgs.m_collisionShapeUniqueId; + } + return -1; +} + + +b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + if (cl) + { + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_CREATE_VISUAL_SHAPE; + command->m_updateFlags =0; + return (b3SharedMemoryCommandHandle) command; + } + return 0; +} + +int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED); + if (status && status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED) + { + return status->m_createVisualShapeResultArgs.m_visualShapeUniqueId; + } + return -1; +} + +b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + if (cl) + { + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_CREATE_MULTI_BODY; + command->m_updateFlags =0; + command->m_createMultiBodyArgs.m_bodyName[0] = 0; + command->m_createMultiBodyArgs.m_baseLinkIndex = -1; + command->m_createMultiBodyArgs.m_numLinks = 0; + return (b3SharedMemoryCommandHandle) command; + } + return 0; +} + + +int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_MULTI_BODY); + if (command->m_type==CMD_CREATE_MULTI_BODY) + { + int numLinks = command->m_createMultiBodyArgs.m_numLinks; + + if (numLinksm_updateFlags |=MULTI_BODY_HAS_BASE; + command->m_createMultiBodyArgs.m_baseLinkIndex = baseLinkIndex; + command->m_createMultiBodyArgs.m_baseWorldPosition[0]=basePosition[0]; + command->m_createMultiBodyArgs.m_baseWorldPosition[1]=basePosition[1]; + command->m_createMultiBodyArgs.m_baseWorldPosition[2]=basePosition[2]; + + command->m_createMultiBodyArgs.m_baseWorldOrientation[0]=baseOrientation[0]; + command->m_createMultiBodyArgs.m_baseWorldOrientation[1]=baseOrientation[1]; + command->m_createMultiBodyArgs.m_baseWorldOrientation[2]=baseOrientation[2]; + command->m_createMultiBodyArgs.m_baseWorldOrientation[3]=baseOrientation[3]; + + command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+0] = mass; + command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+1] = mass; + command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+2] = mass; + + + command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+0] = 0; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+1] = 0; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+2] = 0; + + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+0] = 0; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+1] = 0; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+2] = 0; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+3] = 1; + + command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[baseLinkIndex]= collisionShapeUnique; + command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[baseLinkIndex] = visualShapeUniqueId; + + command->m_createMultiBodyArgs.m_linkMasses[command->m_createMultiBodyArgs.m_numLinks] = mass; + command->m_createMultiBodyArgs.m_numLinks++; + } + return numLinks; + } + return -2; +} + + +int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED); + if (status && status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED) + { + return status->m_createMultiBodyResultArgs.m_bodyUniqueId; + } + return -1; +} + b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index c11ac9d97..62d27faff 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -313,6 +313,28 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl ///the creation of collision shapes and rigid bodies etc is likely going to change, ///but good to have a b3CreateBoxShapeCommandInit for now + +b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient); +void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius); +void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]); + + +void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]); + +int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); + +b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient); +int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); + +b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); +int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]); + +//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId); + +int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle); + + + ///create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1) ///after that, you can optionally adjust the initial position, orientation and size b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 4872a151a..a2061b246 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -442,6 +442,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { break; } + + case CMD_CREATE_MULTI_BODY_COMPLETED: case CMD_URDF_LOADING_COMPLETED: { B3_PROFILE("CMD_URDF_LOADING_COMPLETED"); @@ -1056,6 +1058,28 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Request dynamics info failed"); break; } + case CMD_CREATE_COLLISION_SHAPE_FAILED: + { + b3Warning("Request createCollisionShape failed"); + break; + } + case CMD_CREATE_COLLISION_SHAPE_COMPLETED: + case CMD_CREATE_VISUAL_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_MULTI_BODY_FAILED: + { + b3Warning("Request createMultiBody failed"); + break; + } + case CMD_CREATE_VISUAL_SHAPE_FAILED: + { + b3Warning("Request createVisualShape failed"); + break; + } + default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index e03e2d7f0..ed8376c9f 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -835,6 +835,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } break; } + case CMD_CREATE_MULTI_BODY_COMPLETED: case CMD_URDF_LOADING_COMPLETED: { @@ -881,6 +882,33 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd b3Warning("createConstraint failed"); break; } + + case CMD_CREATE_COLLISION_SHAPE_FAILED: + { + b3Warning("createCollisionShape failed"); + break; + } + case CMD_CREATE_COLLISION_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_VISUAL_SHAPE_FAILED: + { + b3Warning("createVisualShape failed"); + break; + } + case CMD_CREATE_VISUAL_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_MULTI_BODY_FAILED: + { + b3Warning("createMultiBody failed"); + break; + } + default: { //b3Warning("Unknown server status type"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 39a18f909..9dc8a1b9e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -124,7 +124,14 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw } }; - +struct InteralCollisionShapeData +{ + btCollisionShape* m_collisionShape; + void clear() + { + m_collisionShape=0; + } +}; struct InteralBodyData { @@ -171,7 +178,7 @@ struct InteralUserConstraintData }; typedef b3PoolBodyHandle InternalBodyHandle; - +typedef b3PoolBodyHandle InternalCollisionShapeHandle; class btCommandChunk { @@ -1133,7 +1140,7 @@ struct PhysicsServerCommandProcessorInternalData { ///handle management b3ResizablePool< InternalBodyHandle > m_bodyHandles; - + b3ResizablePool m_userCollisionShapeHandles; bool m_allowRealTimeSimulation; @@ -1244,6 +1251,9 @@ struct PhysicsServerCommandProcessorInternalData m_bodyHandles.exitHandles(); m_bodyHandles.initHandles(); + m_userCollisionShapeHandles.exitHandles(); + m_userCollisionShapeHandles.initHandles(); + #if 0 btAlignedObjectArray bla; @@ -1490,6 +1500,211 @@ void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep) } +struct ProgrammaticUrdfInterface : public URDFImporterInterface +{ + int m_bodyUniqueId; + + const b3CreateMultiBodyArgs& m_createBodyArgs; + mutable b3AlignedObjectArray m_allocatedCollisionShapes; + PhysicsServerCommandProcessorInternalData* m_data; + + ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data) + :m_bodyUniqueId(-1), + m_createBodyArgs(bodyArgs), + m_data(data) + { + + } + + virtual ~ProgrammaticUrdfInterface() + { + + } + + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false) + { + b3Assert(0); + return false; + } + + virtual const char* getPathPrefix() + { + return ""; + } + + ///return >=0 for the root link index, -1 if there is no root link + virtual int getRootLinkIndex() const + { + return m_createBodyArgs.m_baseLinkIndex; + } + + ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range) + virtual std::string getLinkName(int linkIndex) const + { + return "link"; + } + + //various derived class in internal source code break with new pure virtual methods, so provide some default implementation + virtual std::string getBodyName() const + { + return m_createBodyArgs.m_bodyName; + } + + /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise + virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const + { + b3Assert(0); + return false; + } + + virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const + { + return false; + } + + virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const + { + return 0; + } + ///this API will likely change, don't override it! + virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const + { + + return false; + } + + virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const + { + b3Assert(0); + return false; + } + + virtual std::string getJointName(int linkIndex) const + { + b3Assert(0); + return "joint"; + } + + //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity. + virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const + { + if (urdfLinkIndex>=0 && urdfLinkIndex < m_createBodyArgs.m_numLinks) + { + mass = m_createBodyArgs.m_linkMasses[urdfLinkIndex]; + localInertiaDiagonal.setValue( + m_createBodyArgs.m_linkInertias[urdfLinkIndex*3+0], + m_createBodyArgs.m_linkInertias[urdfLinkIndex*3+1], + m_createBodyArgs.m_linkInertias[urdfLinkIndex*3+2]); + inertialFrame.setOrigin(btVector3( + m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex*3+0], + m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex*3+1], + m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex*3+2])); + inertialFrame.setRotation(btQuaternion( + m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+0], + m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+1], + m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+2], + m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+3])); + } else + { + mass = 0; + localInertiaDiagonal.setValue(0,0,0); + inertialFrame.setIdentity(); + } + } + + ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed + virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray& childLinkIndices) const + { + } + + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const + { + return false; + }; + + virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const + { + //backwards compatibility for custom file importers + jointMaxForce = 0; + jointMaxVelocity = 0; + return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction); + }; + + virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const + { + rootTransformInWorld.setOrigin(btVector3( + m_createBodyArgs.m_baseWorldPosition[0], + m_createBodyArgs.m_baseWorldPosition[1], + m_createBodyArgs.m_baseWorldPosition[2])); + rootTransformInWorld.setRotation(btQuaternion( + m_createBodyArgs.m_baseWorldOrientation[0], + m_createBodyArgs.m_baseWorldOrientation[1], + m_createBodyArgs.m_baseWorldOrientation[2], + m_createBodyArgs.m_baseWorldOrientation[3])); + return true; + } + virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld) + { + b3Assert(0); + } + + ///quick hack: need to rethink the API/dependencies of this + virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const + { + return -1; + } + + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const + { + } + virtual void setBodyUniqueId(int bodyId) + { + m_bodyUniqueId = bodyId; + } + virtual int getBodyUniqueId() const + { + return m_bodyUniqueId; + } + + //default implementation for backward compatibility + virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const + { + btCompoundShape* compound = new btCompoundShape(); + + int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[linkIndex]; + if (colShapeUniqueId>=0) + { + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId); + if (handle) + { + btTransform childTrans; + childTrans.setIdentity(); + compound->addChildShape(localInertiaFrame.inverse()*childTrans,handle->m_collisionShape); + } + } + m_allocatedCollisionShapes.push_back(compound); + return compound; + } + + virtual int getNumAllocatedCollisionShapes() const + { + return m_allocatedCollisionShapes.size(); + } + + virtual class btCollisionShape* getAllocatedCollisionShape(int index) + { + return m_allocatedCollisionShapes[index]; + } + virtual int getNumModels() const + { + return 1; + } + virtual void activateModel(int /*modelIndex*/) + { + } +}; + + void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() { ///collision configuration contains default setup for memory, collision setup @@ -1829,7 +2044,22 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, createJointMotors(mb); - +#ifdef B3_ENABLE_TINY_AUDIO + { + SDFAudioSource audioSource; + int urdfRootLink = u2b.getRootLinkIndex();//LinkIndex = creation.m_mb2urdfLink[-1]; + if (u2b.getLinkAudioSource(urdfRootLink,audioSource)) + { + int flags = mb->getBaseCollider()->getCollisionFlags(); + mb->getBaseCollider()->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER); + audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str()); + if (audioSource.m_userIndex>=0) + { + bodyHandle->m_audioSources.insert(-1, audioSource); + } + } + } +#endif //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); @@ -1853,14 +2083,33 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, m_data->m_strings.push_back(jointName); mb->getLink(i).m_jointName = jointName->c_str(); + +#ifdef B3_ENABLE_TINY_AUDIO + { + SDFAudioSource audioSource; + int urdfLinkIndex = creation.m_mb2urdfLink[link]; + if (u2b.getLinkAudioSource(urdfLinkIndex,audioSource)) + { + int flags = mb->getLink(link).m_collider->getCollisionFlags(); + mb->getLink(i).m_collider->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER); + audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str()); + if (audioSource.m_userIndex>=0) + { + bodyHandle->m_audioSources.insert(link, audioSource); + } + } + } +#endif } std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); m_data->m_strings.push_back(baseName); mb->setBaseName(baseName->c_str()); } else { - b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); + //b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); bodyHandle->m_rigidBody = rb; + rb->setUserIndex2(bodyUniqueId); + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); } } @@ -1945,6 +2194,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags) { + m_data->m_sdfRecentLoadedBodies.clear(); + *bodyUniqueIdPtr = -1; + BT_PROFILE("loadURDF"); btAssert(m_data->m_dynamicsWorld); if (!m_data->m_dynamicsWorld) @@ -1962,6 +2214,25 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (loadOk) { + +#if 1 + btTransform rootTrans; + rootTrans.setOrigin(pos); + rootTrans.setRotation(orn); + u2b.setRootTransformInWorld(rootTrans); + bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); + if (ok) + { + if (m_data->m_sdfRecentLoadedBodies.size()==1) + { + *bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0]; + + } + m_data->m_sdfRecentLoadedBodies.clear(); + } + return ok; +#else + //get a body index int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); if (bodyUniqueIdPtr) @@ -2132,9 +2403,9 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto return true; } } - + #endif } - + return false; } @@ -3155,6 +3426,147 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_CREATE_COLLISION_SHAPE: + { + hasStatus = true; + serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; + + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + btCollisionShape* shape = 0; + + btCompoundShape* compound = 0; + + if (clientCmd.m_createCollisionShapeArgs.m_numCollisionShapes>1) + { + compound = worldImporter->createCompoundShape(); + } + for (int i=0;icreateCompoundShape(); + } + } + + switch (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_type) + { + case GEOM_SPHERE: + { + double radius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius; + shape = worldImporter->createSphereShape(radius); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + case GEOM_BOX: + { + //double halfExtents[3] = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius; + shape = worldImporter->createBoxShape(btVector3( + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2])); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + } + } + #if 0 + shape = worldImporter->createCylinderShapeX(radius,height); + shape = worldImporter->createCylinderShapeY(radius,height); + shape = worldImporter->createCylinderShapeZ(radius,height); + shape = worldImporter->createCapsuleShapeX(radius,height); + shape = worldImporter->createCapsuleShapeY(radius,height); + shape = worldImporter->createCapsuleShapeZ(radius,height); + shape = worldImporter->createBoxShape(halfExtents); + #endif + if (compound && compound->getNumChildShapes()) + { + shape = compound; + } + + if (shape) + { + int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle(); + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid); + handle->m_collisionShape = shape; + serverStatusOut.m_createCollisionShapeResultArgs.m_collisionShapeUniqueId = collisionShapeUid; + m_data->m_worldImporters.push_back(worldImporter); + serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED; + } else + { + delete worldImporter; + } + + + break; + } + case CMD_CREATE_VISUAL_SHAPE: + { + hasStatus = true; + serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; + break; + } + case CMD_CREATE_MULTI_BODY: + { + hasStatus = true; + serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED; + if (clientCmd.m_createMultiBodyArgs.m_baseLinkIndex>=0) + { + ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); + + bool useMultiBody = false; + int flags = 0; + bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); + + if (ok) + { + int bodyUniqueId = -1; + + if (m_data->m_sdfRecentLoadedBodies.size()==1) + { + bodyUniqueId = m_data->m_sdfRecentLoadedBodies[0]; + } + m_data->m_sdfRecentLoadedBodies.clear(); + if (bodyUniqueId>=0) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED; + + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); + } + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + } + } + + //ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); + + + } + break; + } case CMD_LOAD_URDF: { BT_PROFILE("CMD_LOAD_URDF"); @@ -3193,13 +3605,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm initialPos,initialOrn, useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags); - if (completedOk) + if (completedOk && bodyUniqueId>=0) { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + if (m_data->m_urdfLinkNameMapper.size()) { @@ -6409,8 +6824,8 @@ void PhysicsServerCommandProcessor::resetSimulation() m_data->m_bodyHandles.exitHandles(); m_data->m_bodyHandles.initHandles(); - - + m_data->m_userCollisionShapeHandles.exitHandles(); + m_data->m_userCollisionShapeHandles.initHandles(); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b322c163d..0d89aa9bd 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -761,6 +761,95 @@ struct ConfigureOpenGLVisualizerRequest int m_setEnabled; }; +enum +{ + URDF_GEOM_HAS_RADIUS = 1, +}; + +struct b3CreateCollisionShape +{ + int m_type;//see UrdfGeomTypes + + int m_hasChildTransform; + double m_childPosition[3]; + double m_childOrientation[4]; + + double m_sphereRadius; + double m_boxHalfExtents[3]; + double m_capsuleRadius; + double m_capsuleHeight; + int m_hasFromTo; + double m_capsuleFrom[3]; + double m_capsuleTo[3]; + double m_planeNormal[3]; + + int m_meshFileType; + char m_meshFileName[1024]; + double m_meshScale; +}; + +#define MAX_COMPOUND_COLLISION_SHAPES 128 + +struct b3CreateCollisionShapeArgs +{ + int m_numCollisionShapes; + b3CreateCollisionShape m_shapes[MAX_COMPOUND_COLLISION_SHAPES]; +}; + + +struct b3CreateVisualShapeArgs +{ + int m_visualShapeUniqueId; +}; + +#define MAX_CREATE_MULTI_BODY_LINKS 128 +enum eCreateMultiBodyEnum +{ + MULTI_BODY_HAS_BASE=1, +}; +struct b3CreateMultiBodyArgs +{ + char m_bodyName[1024]; + int m_baseLinkIndex; + + double m_baseWorldPosition[3]; + double m_baseWorldOrientation[4]; + + int m_numLinks; + double m_linkMasses[MAX_CREATE_MULTI_BODY_LINKS]; + double m_linkInertias[MAX_CREATE_MULTI_BODY_LINKS*3]; + double m_linkInertialFramePositions[MAX_CREATE_MULTI_BODY_LINKS*3]; + double m_linkInertialFrameOrientations[MAX_CREATE_MULTI_BODY_LINKS*4]; + int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkCollisionShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkVisualShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS]; + + #if 0 + std::string m_name; + std::string m_sourceFile; + btTransform m_rootTransformInWorld; + btHashMap m_materials; + btHashMap m_links; + btHashMap m_joints; + #endif +}; + +struct b3CreateCollisionShapeResultArgs +{ + int m_collisionShapeUniqueId; +}; + +struct b3CreateVisualShapeResultArgs +{ + int m_visualShapeUniqueId; +}; + +struct b3CreateMultiBodyResultArgs +{ + int m_bodyUniqueId; +}; + + struct SharedMemoryCommand { int m_type; @@ -808,6 +897,9 @@ struct SharedMemoryCommand struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments; struct b3ObjectArgs m_removeObjectArgs; struct b3Profile m_profile; + struct b3CreateCollisionShapeArgs m_createCollisionShapeArgs; + struct b3CreateVisualShapeArgs m_createVisualShapeArgs; + struct b3CreateMultiBodyArgs m_createMultiBodyArgs; }; }; @@ -874,6 +966,9 @@ struct SharedMemoryStatus struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs; struct b3ObjectArgs m_removeObjectArgs; struct b3DynamicsInfo m_dynamicsInfo; + struct b3CreateCollisionShapeResultArgs m_createCollisionShapeResultArgs; + struct b3CreateVisualShapeResultArgs m_createVisualShapeResultArgs; + struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index a6b33717b..e5b288dbb 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,6 +7,7 @@ #define SHARED_MEMORY_MAGIC_NUMBER 201706001 //#define SHARED_MEMORY_MAGIC_NUMBER 201703024 + enum EnumSharedMemoryClientCommand { CMD_LOAD_SDF, @@ -60,6 +61,9 @@ enum EnumSharedMemoryClientCommand CMD_CHANGE_DYNAMICS_INFO, CMD_GET_DYNAMICS_INFO, CMD_PROFILE_TIMING, + CMD_CREATE_COLLISION_SHAPE, + CMD_CREATE_VISUAL_SHAPE, + CMD_CREATE_MULTI_BODY, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -145,6 +149,12 @@ enum EnumSharedMemoryServerStatus CMD_REMOVE_BODY_FAILED, CMD_GET_DYNAMICS_INFO_COMPLETED, CMD_GET_DYNAMICS_INFO_FAILED, + CMD_CREATE_COLLISION_SHAPE_FAILED, + CMD_CREATE_COLLISION_SHAPE_COMPLETED, + CMD_CREATE_VISUAL_SHAPE_FAILED, + CMD_CREATE_VISUAL_SHAPE_COMPLETED, + CMD_CREATE_MULTI_BODY_FAILED, + CMD_CREATE_MULTI_BODY_COMPLETED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -204,6 +214,7 @@ struct b3JointInfo double m_jointAxis[3]; // joint axis in parent local frame }; + struct b3UserConstraint { int m_parentBodyIndex; @@ -537,4 +548,17 @@ enum eURDF_Flags URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, }; +enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes +{ + GEOM_SPHERE=2, + GEOM_BOX, + GEOM_CYLINDER, + GEOM_MESH, + GEOM_PLANE, + GEOM_CAPSULE, //non-standard URDF? + GEOM_UNKNOWN, +}; + + + #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/pybullet/examples/createSphereMultiBodies.py b/examples/pybullet/examples/createSphereMultiBodies.py new file mode 100644 index 000000000..3c66c4519 --- /dev/null +++ b/examples/pybullet/examples/createSphereMultiBodies.py @@ -0,0 +1,30 @@ +import pybullet as p +import time + +p.connect(p.GUI) +sphereRadius = 0.05 + +colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) +colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) + +mass = 1 +visualShapeId = -1 + + +for i in range (10): + for j in range (10): + for k in range (10): + if (k&2): + sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1]) + else: + sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1]) + #p.changeDynamics(sphereUid,-1,spinningFriction=0.1, rollingFriction=0.1) +#p.loadSDF("stadium.sdf") +p.loadURDF("plane.urdf", useMaximalCoordinates=0) + +p.setGravity(0,0,-10) +p.setRealTimeSimulation(1) + +while (1): + time.sleep(0.01) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b1a6653b5..124a22297 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4701,6 +4701,152 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* return NULL; } +static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + int shapeType=-1; + double radius=-1; + PyObject* halfExtentsObj=0; + + static char* kwlist[] = {"shapeType","radius","halfExtents", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOi", kwlist, + &shapeType, &radius,&halfExtentsObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (shapeType>=GEOM_SPHERE) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CreateCollisionShapeCommandInit(sm); + if (shapeType==GEOM_SPHERE && radius>0) + { + b3CreateCollisionShapeAddSphere(commandHandle,radius); + } + if (shapeType==GEOM_BOX && halfExtentsObj) + { + double halfExtents[3] = {1,1,1}; + pybullet_internalSetVectord(halfExtentsObj,halfExtents); + b3CreateCollisionShapeAddBox(commandHandle,halfExtents); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED) + { + int uid = b3GetStatusCollisionShapeUniqueId(statusHandle); + PyObject* ob = PyLong_FromLong(uid); + return ob; + } + } + PyErr_SetString(SpamError, "createCollisionShape failed."); + return NULL; +} +#if 0 +b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient); +int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); + +b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient); +int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); + +b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); +int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle); +#endif + + +static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + int test=-1; + + static char* kwlist[] = {"test",NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &test, + &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (test>=0) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CreateVisualShapeCommandInit(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_VISUAL_SHAPE_COMPLETED) + { + int uid = b3GetStatusVisualShapeUniqueId(statusHandle); + PyObject* ob = PyLong_FromLong(uid); + return ob; + } + } + + PyErr_SetString(SpamError, "createVisualShape failed."); + return NULL; +} + +static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + double baseMass = 0; + int baseCollisionShapeIndex=-1; + int baseVisualShapeIndex=-1; + PyObject* basePosObj=0; + PyObject* baseOrnObj=0; + + + static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation", +// "linkParentIndices", "linkJointTypes","linkMasses","linkCollisionShapeIndices", + NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOi", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, + &basePosObj, &baseOrnObj,&physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CreateMultiBodyCommandInit(sm); + double basePosition[3]={0,0,0}; + double baseOrientation[4]={0,0,0,1}; + pybullet_internalSetVectord(basePosObj,basePosition); + pybullet_internalSetVector4d(baseOrnObj,baseOrientation); + int baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) + { + int uid = b3GetStatusMultiBodyUniqueId(statusHandle); + PyObject* ob = PyLong_FromLong(uid); + return ob; + } + } + PyErr_SetString(SpamError, "createMultiBody failed."); + return NULL; +} + + static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryCommandHandle commandHandle; @@ -6218,6 +6364,16 @@ static PyMethodDef SpamMethods[] = { {"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS, "Load multibodies from an MJCF file."}, + {"createCollisionShape", (PyCFunction)pybullet_createCollisionShape, METH_VARARGS | METH_KEYWORDS, + "Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."}, + + {"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS, + "Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."}, + + {"createMultiBody", (PyCFunction)pybullet_createMultiBody, METH_VARARGS | METH_KEYWORDS, + "Create a multi body. Returns a non-negative (int) unique id, if successfull, negative otherwise."}, + + {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, "Create a constraint between two bodies. Returns a (int) unique id, if successfull."}, @@ -6652,6 +6808,15 @@ initpybullet(void) PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT); PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN); + PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE); + PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX); + PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER); + PyModule_AddIntConstant(m, "GEOM_MESH", GEOM_MESH); + PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE); + PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE); + + + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); Py_INCREF(SpamError); PyModule_AddObject(m, "error", SpamError);