From 1d1c822d5238bceb3c1f974307bc837179f6da5b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 2 Jun 2017 16:25:28 -0700 Subject: [PATCH 1/4] btThreadsAreRunning wasn't defined if BT_THREADSAFE was not defined, causing compile errors using premake --- src/LinearMath/btThreads.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/LinearMath/btThreads.h b/src/LinearMath/btThreads.h index b592ec3e5..284702d71 100644 --- a/src/LinearMath/btThreads.h +++ b/src/LinearMath/btThreads.h @@ -79,6 +79,7 @@ const unsigned int BT_MAX_THREAD_COUNT = 64; SIMD_FORCE_INLINE void btMutexLock( btSpinMutex* ) {} SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* ) {} SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* ) {return true;} +SIMD_FORCE_INLINE bool btThreadsAreRunning() { return false;} #endif // From b23cb1dd2c838e815402fc0fdaf77325e1834a1e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 3 Jun 2017 10:57:56 -0700 Subject: [PATCH 2/4] 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); From a7aed37632ad7778d6ba587e673a67e0e789cb71 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 4 Jun 2017 22:04:16 -0700 Subject: [PATCH 3/4] work on pybullet/C-API createMultiBody (still preliminary, only sphere/box collision shapes, no links/hierarchies yet, soon) pybullet/C-API, expose linear/angular damping fix some warnings (param name needs to be same in .h and .cpp) fix potential startup threading issue (args were deleted in main thread while still possibly use in child thread) fix for spinning/rolling friction in case of mixing maximal and reduced coordinate btMultiBody+btRigidBody --- examples/ExampleBrowser/OpenGLGuiHelper.h | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 36 +++++++++ examples/SharedMemory/PhysicsClientC_API.h | 8 +- .../PhysicsServerCommandProcessor.cpp | 29 ++++++- examples/SharedMemory/SharedMemoryCommands.h | 5 ++ .../SharedMemoryInProcessPhysicsC_API.cpp | 16 ++-- .../TinyRendererVisualShapeConverter.h | 2 +- .../examples/createSphereMultiBodies.py | 24 +++--- examples/pybullet/pybullet.c | 38 +++++++-- .../Dynamics/btSimulationIslandManagerMt.h | 2 +- .../Featherstone/btMultiBody.cpp | 10 +-- .../btMultiBodyConstraintSolver.cpp | 79 +++++++++++-------- 12 files changed, 183 insertions(+), 68 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index bdb885a2c..ccc935dcb 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -48,7 +48,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ); - virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float camTarget[3]) const; + virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const; virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index c8b29a115..2ec671d6d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -877,6 +877,17 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass return -2; } +//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet +void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_MULTI_BODY); + if (command->m_type==CMD_CREATE_MULTI_BODY) + { + command->m_updateFlags |= MULT_BODY_USE_MAXIMAL_COORDINATES; + } +} int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle) { @@ -1269,6 +1280,11 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) bodyId = status->m_rigidBodyCreateArgs.m_bodyUniqueId; break; } + case CMD_CREATE_MULTI_BODY_COMPLETED: + { + bodyId = status->m_dataStreamArguments.m_bodyUniqueId; + break; + } default: { b3Assert(0); @@ -1541,9 +1557,29 @@ int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle command->m_changeDynamicsInfoArgs.m_restitution = restitution; command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_RESTITUTION; return 0; +} +int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linearDamping = linearDamping; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING; + return 0; } + +int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linearDamping = angularDamping; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING; + return 0; +} + b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 62d27faff..441395228 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -87,7 +87,8 @@ int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHa int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution); - +int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping); +int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping); b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); @@ -329,9 +330,10 @@ 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); +//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet +void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle); -int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle); +//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9dc8a1b9e..40385d85c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3530,9 +3530,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED; if (clientCmd.m_createMultiBodyArgs.m_baseLinkIndex>=0) { + m_data->m_sdfRecentLoadedBodies.clear(); + ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); - bool useMultiBody = false; + bool useMultiBody = true; + if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) + { + useMultiBody = false; + } + int flags = 0; bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); @@ -4329,6 +4336,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { mb->getBaseCollider()->setRestitution(restitution); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) + { + mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) { mb->getBaseCollider()->setFriction(lateralFriction); @@ -4395,6 +4411,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (body && body->m_rigidBody) { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) + { + btScalar angDamping = body->m_rigidBody->getAngularDamping(); + body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping,angDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + btScalar linDamping = body->m_rigidBody->getLinearDamping(); + body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) { body->m_rigidBody->setRestitution(restitution); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 0d89aa9bd..bde429122 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -117,6 +117,8 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION=8, CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION=16, CHANGE_DYNAMICS_INFO_SET_RESTITUTION=32, + CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING=64, + CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING=128, }; struct ChangeDynamicsInfoArgs @@ -129,6 +131,8 @@ struct ChangeDynamicsInfoArgs double m_spinningFriction; double m_rollingFriction; double m_restitution; + double m_linearDamping; + double m_angularDamping; }; struct GetDynamicsInfoArgs @@ -806,6 +810,7 @@ struct b3CreateVisualShapeArgs enum eCreateMultiBodyEnum { MULTI_BODY_HAS_BASE=1, + MULT_BODY_USE_MAXIMAL_COORDINATES=2, }; struct b3CreateMultiBodyArgs { diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 633441c95..8b5200f6b 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -93,22 +93,23 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int arg class InProcessPhysicsClientSharedMemory : public PhysicsClientSharedMemory { btInProcessExampleBrowserInternalData* m_data; -public: + char** m_newargv; +public: + InProcessPhysicsClientSharedMemory(int argc, char* argv[]) { int newargc = argc+2; - char** newargv = (char**)malloc(sizeof(void*)*newargc); + m_newargv = (char**)malloc(sizeof(void*)*newargc); for (int i=0;i=0) + { + b3ChangeDynamicsInfoSetLinearDamping(command,bodyUniqueId, linearDamping); + } + if (angularDamping>=0) + { + b3ChangeDynamicsInfoSetAngularDamping(command,bodyUniqueId,angularDamping); + } + + if (restitution>=0) { b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, restitution); @@ -971,6 +983,7 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw int numBodies = 0; int i; int bodyIndicesOut[MAX_SDF_BODIES]; + int useMaximalCoordinates = -1; PyObject* pylist = 0; b3SharedMemoryStatusHandle statusHandle; int statusType; @@ -978,8 +991,8 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"sdfFileName", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &sdfFileName, &physicsClientId)) + static char* kwlist[] = {"sdfFileName", "useMaximalCoordinates", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &sdfFileName, &useMaximalCoordinates, &physicsClientId)) { return NULL; } @@ -991,6 +1004,11 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw } commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); + if (useMaximalCoordinates>0) + { + b3LoadSdfCommandSetUseMultiBody(commandHandle,0); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType != CMD_SDF_LOADING_COMPLETED) @@ -4805,15 +4823,17 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje double baseMass = 0; int baseCollisionShapeIndex=-1; int baseVisualShapeIndex=-1; + int useMaximalCoordinates = 0; PyObject* basePosObj=0; PyObject* baseOrnObj=0; static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation", // "linkParentIndices", "linkJointTypes","linkMasses","linkCollisionShapeIndices", +"useMaximalCoordinates","physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOi", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, - &basePosObj, &baseOrnObj,&physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, + &basePosObj, &baseOrnObj,&useMaximalCoordinates, &physicsClientId)) { return NULL; } @@ -4833,11 +4853,15 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje pybullet_internalSetVectord(basePosObj,basePosition); pybullet_internalSetVector4d(baseOrnObj,baseOrientation); int baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); + if (useMaximalCoordinates>0) + { + b3CreateMultiBodyUseMaximalCoordinates(commandHandle,useMaximalCoordinates); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) { - int uid = b3GetStatusMultiBodyUniqueId(statusHandle); + int uid = b3GetStatusBodyIndex(statusHandle); PyObject* ob = PyLong_FromLong(uid); return ob; } diff --git a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h index e3e0ea523..9a781aaef 100644 --- a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h +++ b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h @@ -59,7 +59,7 @@ public: ) = 0; }; typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray* islands, IslandCallback* callback ); - static void serialIslandDispatch( btAlignedObjectArray* islands, IslandCallback* callback ); + static void serialIslandDispatch( btAlignedObjectArray* islandsPtr, IslandCallback* callback ); static void parallelIslandDispatch( btAlignedObjectArray* islandsPtr, IslandCallback* callback ); protected: btAlignedObjectArray m_allocatedIslands; // owner of all Islands diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index ca9652f1e..c4c3fdea7 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -2013,10 +2013,10 @@ const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali } mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; - // Fill padding with zeros to appease msan. -#ifdef BT_USE_DOUBLE_PRECISION - memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); -#endif - + // Fill padding with zeros to appease msan. +#ifdef BT_USE_DOUBLE_PRECISION + memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); +#endif + return btMultiBodyDataName; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 2aa62e5e5..1e2d07409 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -665,12 +665,12 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); - btVector3 torqueAxis0 = constraintNormal; + btVector3 torqueAxis0 = -constraintNormal; solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = btVector3(0,0,0); } else { - btVector3 torqueAxis0 = constraintNormal; + btVector3 torqueAxis0 = -constraintNormal; solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = btVector3(0,0,0); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); @@ -708,21 +708,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis1 = constraintNormal; - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; solverConstraint.m_contactNormal2 = -btVector3(0,0,0); } else { btVector3 torqueAxis1 = constraintNormal; - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; solverConstraint.m_contactNormal2 = -btVector3(0,0,0); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); } { - btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; btScalar* jacB = 0; @@ -745,8 +744,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { if (rb0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + constraintNormal.dot(vec); + btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); + denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal); } } if (multiBodyB) @@ -765,8 +764,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { if (rb1) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + constraintNormal.dot(vec); + btVector3 iMJaB = rb1?rb1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); + denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal); } } @@ -808,7 +807,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { if (rb0) { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + rel_vel += solverConstraint.m_contactNormal1.dot(rb0?solverBodyA->m_linearVelocity+solverBodyA->m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(rb0?solverBodyA->m_angularVelocity:btVector3(0,0,0)); + } } if (multiBodyB) @@ -822,7 +824,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { if (rb1) { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + rel_vel += solverConstraint.m_contactNormal2.dot(rb1?solverBodyB->m_linearVelocity+solverBodyB->m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(rb1?solverBodyB->m_angularVelocity:btVector3(0,0,0)); + } } @@ -844,12 +849,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { - btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + btScalar velocityError = 0 - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - if (penetration>0) - { - velocityError -= penetration / infoGlobal.m_timeStep; - } + btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv; @@ -1021,6 +1023,33 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// + + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + cp.m_lateralFrictionDir1.normalize(); + cp.m_lateralFrictionDir2.normalize(); + + if (rollingFriction > 0 ) + { + if (cp.m_combinedSpinningFriction>0) + { + addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); + } + if (cp.m_combinedRollingFriction>0) + { + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + + if (cp.m_lateralFrictionDir1.length()>0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + + if (cp.m_lateralFrictionDir2.length()>0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + } + rollingFriction--; + } if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) {/* cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; @@ -1045,26 +1074,12 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else */ { - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - if (rollingFriction > 0 ) - { - if (cp.m_combinedSpinningFriction>0) - { - addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); - } - if (cp.m_combinedRollingFriction>0) - { - - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - } - rollingFriction--; - } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { From 704269afe12de884a765dc7314118db0dfa62e60 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 4 Jun 2017 22:24:14 -0700 Subject: [PATCH 4/4] don't pass second parameter, pybullet func --- examples/pybullet/pybullet.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 6eddb9b4e..1f9c03dd8 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4855,7 +4855,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje int baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); if (useMaximalCoordinates>0) { - b3CreateMultiBodyUseMaximalCoordinates(commandHandle,useMaximalCoordinates); + b3CreateMultiBodyUseMaximalCoordinates(commandHandle); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle);