From 47efe6601763203da21a65f044861fd60c28c9f5 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 21 Sep 2018 09:55:57 -0700 Subject: [PATCH 1/4] disable eglPlugin from setup.py / pip pybullet on Windows and Mac (only enable on Linux) --- .../plugins/eglPlugin/eglRendererPlugin.cpp | 36 ++++++++++++++++++- setup.py | 33 ++++++++++------- 2 files changed, 56 insertions(+), 13 deletions(-) diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp index 1bcfc6341..b130bbf99 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp @@ -62,6 +62,28 @@ B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugi #ifdef EGL_ADD_PYTHON_INIT + +static PyMethodDef eglMethods[] = { + {NULL, NULL, 0, NULL} /* Sentinel */ +}; + +#if PY_MAJOR_VERSION >= 3 +static struct PyModuleDef moduledef = { + PyModuleDef_HEAD_INIT, "eglRenderer", /* m_name */ + "eglRenderer for PyBullet " + , /* m_doc */ + -1, /* m_size */ + eglMethods, /* m_methods */ + NULL, /* m_reload */ + NULL, /* m_traverse */ + NULL, /* m_clear */ + NULL, /* m_free */ +}; +#endif + + + + PyMODINIT_FUNC #if PY_MAJOR_VERSION >= 3 PyInit_eglRenderer(void) @@ -69,8 +91,20 @@ PyInit_eglRenderer(void) initeglRenderer(void) #endif { + + PyObject* m; #if PY_MAJOR_VERSION >= 3 - return 0; + m = PyModule_Create(&moduledef); +#else + m = Py_InitModule3("eglRenderer", eglMethods, "eglRenderer for PyBullet"); #endif + +#if PY_MAJOR_VERSION >= 3 + if (m == NULL) return m; +#else + if (m == NULL) return; +#endif + + } #endif //EGL_ADD_PYTHON_INIT diff --git a/setup.py b/setup.py index ae7e7b4bf..11c383f46 100644 --- a/setup.py +++ b/setup.py @@ -42,7 +42,7 @@ CXX_FLAGS += '-DBT_USE_DOUBLE_PRECISION ' CXX_FLAGS += '-DBT_ENABLE_ENET ' CXX_FLAGS += '-DBT_ENABLE_CLSOCKET ' CXX_FLAGS += '-DB3_DUMP_PYTHON_VERSION ' - +CXX_FLAGS += '-DEGL_ADD_PYTHON_INIT ' EGL_CXX_FLAGS = '' @@ -535,12 +535,26 @@ print("packages") print(find_packages('examples/pybullet/gym')) print("-----") -eglRender = Extension("eglRenderer", - sources = egl_renderer_sources, +extensions = [] + +pybullet_ext = Extension("pybullet", + sources = sources, libraries = libraries, - extra_compile_args=(CXX_FLAGS+EGL_CXX_FLAGS ).split(), - include_dirs = include_dirs + ["src","examples", "examples/ThirdPartyLibs","examples/ThirdPartyLibs/glad", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"] + extra_compile_args=CXX_FLAGS.split(), + include_dirs = include_dirs + ["src","examples/ThirdPartyLibs","examples/ThirdPartyLibs/glad", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"] ) +extensions.append(pybullet_ext) + + +if 'BT_USE_EGL' in EGL_CXX_FLAGS: + + eglRender = Extension("eglRenderer", + sources = egl_renderer_sources, + libraries = libraries, + extra_compile_args=(CXX_FLAGS+EGL_CXX_FLAGS ).split(), + include_dirs = include_dirs + ["src","examples", "examples/ThirdPartyLibs","examples/ThirdPartyLibs/glad", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"]) + + extensions.append(eglRender) setup( @@ -554,13 +568,8 @@ setup( license='zlib', platforms='any', keywords=['game development', 'virtual reality', 'physics simulation', 'robotics', 'collision detection', 'opengl'], - ext_modules = [eglRender, Extension("pybullet", - sources = sources, - libraries = libraries, - extra_compile_args=CXX_FLAGS.split(), - include_dirs = include_dirs + ["src","examples/ThirdPartyLibs","examples/ThirdPartyLibs/glad", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"] - ) ], - classifiers=['Development Status :: 5 - Production/Stable', + ext_modules = extensions, + classifiers=['Development Status :: 5 - Production/Stable', 'License :: OSI Approved :: zlib/libpng License', 'Operating System :: Microsoft :: Windows', 'Operating System :: POSIX :: Linux', From 278454ae17d01a8878dc0d3c7012fc0adabf4c7a Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 21 Sep 2018 17:33:13 -0700 Subject: [PATCH 2/4] allow to disable self-collision for a link-pair --- .../collisionFilterPlugin.cpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp index 4e65cfcbc..3ec2a99ca 100644 --- a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp +++ b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp @@ -62,6 +62,13 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB); b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); } + if (objectUniqueIdA==objectUniqueIdB) + { + if (keyValue.m_linkIndexA>keyValue.m_linkIndexB) + { + b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); + } + } m_customCollisionFilters.insert(keyValue,keyValue); @@ -83,6 +90,13 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB); b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); } + if (objectUniqueIdA==objectUniqueIdB) + { + if (keyValue.m_linkIndexA>keyValue.m_linkIndexB) + { + b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); + } + } m_customCollisionFilters.remove(keyValue); } @@ -116,6 +130,13 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB); b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); } + if (objectUniqueIdA==objectUniqueIdB) + { + if (keyValue.m_linkIndexA>keyValue.m_linkIndexB) + { + b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); + } + } b3CustomCollisionFilter* filter = m_customCollisionFilters.find(keyValue); if (filter) From cdf8c908ad975003a1e4ad052f835baf94fc88ab Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 22 Sep 2018 13:17:09 -0700 Subject: [PATCH 3/4] add getClosestPoints.py example. allow to perform a getClosestPoints query with a collisionShape and world transform (position, orientation) that isn't part of the world. (use createCollisionShape to create it) --- examples/SharedMemory/PhysicsClientC_API.cpp | 68 +++++++++++++- examples/SharedMemory/PhysicsClientC_API.h | 9 ++ .../PhysicsServerCommandProcessor.cpp | 94 +++++++++++++++++-- examples/SharedMemory/SharedMemoryCommands.h | 16 ++++ .../pybullet/examples/getClosestPoints.py | 38 ++++++++ examples/pybullet/pybullet.c | 59 +++++++++++- 6 files changed, 270 insertions(+), 14 deletions(-) create mode 100644 examples/pybullet/examples/getClosestPoints.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 373277fe3..8d35be32d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3883,10 +3883,76 @@ B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle com struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); - command->m_updateFlags += CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD; + command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD; command->m_requestContactPointArguments.m_closestDistanceThreshold = distance; } +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeA(b3SharedMemoryCommandHandle commandHandle, int collisionShapeA) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A; + command->m_requestContactPointArguments.m_collisionShapeA = collisionShapeA; +} + +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeB(b3SharedMemoryCommandHandle commandHandle, int collisionShapeB) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B; + command->m_requestContactPointArguments.m_collisionShapeB = collisionShapeB; +} + +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionA(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionA[/*3*/]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A; + command->m_requestContactPointArguments.m_collisionShapePositionA[0] = collisionShapePositionA[0]; + command->m_requestContactPointArguments.m_collisionShapePositionA[1] = collisionShapePositionA[1]; + command->m_requestContactPointArguments.m_collisionShapePositionA[2] = collisionShapePositionA[2]; +} + +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionB(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionB[/*3*/]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B; + command->m_requestContactPointArguments.m_collisionShapePositionB[0] = collisionShapePositionB[0]; + command->m_requestContactPointArguments.m_collisionShapePositionB[1] = collisionShapePositionB[1]; + command->m_requestContactPointArguments.m_collisionShapePositionB[2] = collisionShapePositionB[2]; +} + +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationA(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationA[/*4*/]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A; + command->m_requestContactPointArguments.m_collisionShapeOrientationA[0] = collisionShapeOrientationA[0]; + command->m_requestContactPointArguments.m_collisionShapeOrientationA[1] = collisionShapeOrientationA[1]; + command->m_requestContactPointArguments.m_collisionShapeOrientationA[2] = collisionShapeOrientationA[2]; + command->m_requestContactPointArguments.m_collisionShapeOrientationA[3] = collisionShapeOrientationA[3]; +} + + +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationB(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationB[/*4*/]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B; + command->m_requestContactPointArguments.m_collisionShapeOrientationB[0] = collisionShapeOrientationB[0]; + command->m_requestContactPointArguments.m_collisionShapeOrientationB[1] = collisionShapeOrientationB[1]; + command->m_requestContactPointArguments.m_collisionShapeOrientationB[2] = collisionShapeOrientationB[2]; + command->m_requestContactPointArguments.m_collisionShapeOrientationB[3] = collisionShapeOrientationB[3]; +} + + ///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates) B3_SHARED_API b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3]) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 964f87c83..5bdbdaf7b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -290,6 +290,15 @@ B3_SHARED_API void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle c B3_SHARED_API void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); B3_SHARED_API void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance); + +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeA(b3SharedMemoryCommandHandle commandHandle, int collisionShapeA); +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeB(b3SharedMemoryCommandHandle commandHandle, int collisionShapeB); +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionA(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionA[/*3*/]); +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionB(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionB[/*3*/]); +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationA(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationA[/*4*/]); +B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationB(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationB[/*4*/]); + + B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo); ///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5b32d13a3..70c216959 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1693,6 +1693,7 @@ struct PhysicsServerCommandProcessorInternalData b3HashMap m_cachedVUrdfisualShapes; b3ThreadPool* m_threadPool; + btScalar m_defaultCollisionMargin; PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc) :m_pluginManager(proc), @@ -1725,7 +1726,8 @@ struct PhysicsServerCommandProcessorInternalData m_pdControlPlugin(-1), m_collisionFilterPlugin(-1), m_grpcPlugin(-1), - m_threadPool(0) + m_threadPool(0), + m_defaultCollisionMargin(0.001) { { @@ -3965,8 +3967,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str { bool hasStatus = true; serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; - btScalar defaultCollisionMargin = 0.001; - + btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld); btCollisionShape* shape = 0; @@ -3977,6 +3978,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str if (clientCmd.m_createUserShapeArgs.m_numUserShapes>1) { compound = worldImporter->createCompoundShape(); + compound ->setMargin(m_data->m_defaultCollisionMargin); } for (int i = 0; i < clientCmd.m_createUserShapeArgs.m_numUserShapes; i++) { @@ -4015,6 +4017,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str { double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius; shape = worldImporter->createSphereShape(radius); + shape->setMargin(m_data->m_defaultCollisionMargin); if (compound) { compound->addChildShape(childTransform, shape); @@ -4031,6 +4034,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1], clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]); shape = worldImporter->createBoxShape(halfExtents); + shape->setMargin(m_data->m_defaultCollisionMargin); if (compound) { compound->addChildShape(childTransform, shape); @@ -4043,6 +4047,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str { shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); + shape->setMargin(m_data->m_defaultCollisionMargin); if (compound) { compound->addChildShape(childTransform, shape); @@ -4057,6 +4062,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str { shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, 0.5*clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); + shape->setMargin(m_data->m_defaultCollisionMargin); if (compound) { compound->addChildShape(childTransform, shape); @@ -4074,6 +4080,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]); shape = worldImporter->createPlaneShape(planeNormal, 0); + shape->setMargin(m_data->m_defaultCollisionMargin); if (compound) { compound->addChildShape(childTransform, shape); @@ -4145,12 +4152,12 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str { compound = worldImporter->createCompoundShape(); } - compound->setMargin(defaultCollisionMargin); + compound->setMargin(m_data->m_defaultCollisionMargin); for (int s = 0; s < (int)shapes.size(); s++) { btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); - convexHull->setMargin(defaultCollisionMargin); + convexHull->setMargin(m_data->m_defaultCollisionMargin); tinyobj::shape_t& shape = shapes[s]; int faceCount = shape.mesh.indices.size(); @@ -4253,6 +4260,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str if (compound) { compound->addChildShape(childTransform, shape); + shape->setMargin(m_data->m_defaultCollisionMargin); } } delete glmesh; @@ -4265,11 +4273,11 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str { compound = worldImporter->createCompoundShape(); } - compound->setMargin(defaultCollisionMargin); + compound->setMargin(m_data->m_defaultCollisionMargin); { btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); - convexHull->setMargin(defaultCollisionMargin); + convexHull->setMargin(m_data->m_defaultCollisionMargin); for (int v = 0; v < convertedVerts.size(); v++) { @@ -6235,7 +6243,77 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand btAlignedObjectArray setB; btAlignedObjectArray setALinkIndex; btAlignedObjectArray setBLinkIndex; - + + btCollisionObject colObA; + btCollisionObject colObB; + + int collisionShapeA = (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A) ? + clientCmd.m_requestContactPointArguments.m_collisionShapeA : -1; + int collisionShapeB = (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B) ? + clientCmd.m_requestContactPointArguments.m_collisionShapeB : -1; + + if (collisionShapeA>=0) + { + btVector3 posA(0,0,0); + if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A) + { + posA.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[0], + clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[1], + clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[2]); + } + btQuaternion ornA(0,0,0,1); + if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A) + { + ornA.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[0], + clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[1], + clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[2], + clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[3]); + } + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeA); + if (handle && handle->m_collisionShape) + { + colObA.setCollisionShape(handle->m_collisionShape); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posA); + tr.setRotation(ornA); + colObA.setWorldTransform(tr); + setA.push_back(&colObA); + setALinkIndex.push_back(-2); + } + } + if (collisionShapeB>=0) + { + btVector3 posB(0,0,0); + if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B) + { + posB.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[0], + clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[1], + clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[2]); + } + btQuaternion ornB(0,0,0,1); + if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B) + { + ornB.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[0], + clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[1], + clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[2], + clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[3]); + } + + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeB); + if (handle && handle->m_collisionShape) + { + colObB.setCollisionShape(handle->m_collisionShape); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posB); + tr.setRotation(ornB); + colObB.setWorldTransform(tr); + setB.push_back(&colObB); + setBLinkIndex.push_back(-2); + } + } + if (bodyUniqueIdA >= 0) { InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 90a79331f..f48c4b817 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -278,6 +278,14 @@ enum EnumRequestContactDataUpdateFlags CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD=2, CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER = 4, CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER = 8, + + CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A = 16, + CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B = 32, + CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A = 64, + CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B = 128, + CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A= 256, + CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B= 512, + }; struct RequestRaycastIntersections @@ -309,6 +317,14 @@ struct RequestContactDataArgs int m_linkIndexAIndexFilter; int m_linkIndexBIndexFilter; double m_closestDistanceThreshold; + + int m_collisionShapeA; + int m_collisionShapeB; + double m_collisionShapePositionA[3]; + double m_collisionShapePositionB[3]; + double m_collisionShapeOrientationA[4]; + double m_collisionShapeOrientationB[4]; + int m_mode; }; diff --git a/examples/pybullet/examples/getClosestPoints.py b/examples/pybullet/examples/getClosestPoints.py new file mode 100644 index 000000000..04c72bf42 --- /dev/null +++ b/examples/pybullet/examples/getClosestPoints.py @@ -0,0 +1,38 @@ +import pybullet as p +import time +p.connect(p.GUI) +useCollisionShapeQuery = False + +geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1) +geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2,0.2,0.2]) +obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom,basePosition=[0.5,0,1]) +baseOrientationB = p.getQuaternionFromEuler([0,0.3,0])#[0,0.5,0.5,0] +basePositionB = [1.5,0,1] +obB = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geomBox,basePosition=basePositionB,baseOrientation=baseOrientationB ) + +lineWidth=3 +colorRGB=[1,0,0] +lineId=p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0,0],lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0) +pitch=0 +while (p.isConnected()): + pitch += 0.01 + if (pitch>=3.1415*2.): + pitch=0 + + baseOrientationB = p.getQuaternionFromEuler([0,pitch,0])#[0,0.5,0.5,0] + p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB) + + if (useCollisionShapeQuery): + pts = p.getClosestPoints(bodyA=-1, bodyB=-1, distance=100, collisionShapeA=geom,collisionShapeB=geomBox, collisionShapePositionA=[0.5,0,1],collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB) + else: + pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100) + + if len(pts)>0: + #print(pts) + distance = pts[0][8] + #print("distance=",distance) + ptA = pts[0][5] + ptB = pts[0][6] + p.addUserDebugLine(lineFromXYZ=ptA,lineToXYZ=ptB,lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0,replaceItemUniqueId=lineId); + time.sleep(1./240.) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d3ca5fb96..a5c5bb6f6 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6002,6 +6002,17 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py int bodyUniqueIdB = -1; int linkIndexA = -2; int linkIndexB = -2; + int collisionShapeA = -1; + int collisionShapeB = -1; + + PyObject* collisionShapePositionAObj = 0; + PyObject* collisionShapeOrientationAObj = 0; + double collisionShapePositionA[3]={0,0,0}; + double collisionShapeOrientationA[4]={0,0,0,1}; + PyObject* collisionShapePositionBObj = 0; + PyObject* collisionShapeOrientationBObj = 0; + double collisionShapePositionB[3]={0,0,0}; + double collisionShapeOrientationB[4]={0,0,0,1}; double distanceThreshold = 0.f; @@ -6011,10 +6022,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py int statusType; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "physicsClientId", NULL}; + static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "collisionShapeA","collisionShapeB", "collisionShapePositionA","collisionShapePositionB","collisionShapeOrientationA","collisionShapeOrientationB","physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iii", kwlist, - &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iiiiOOOOi", kwlist, + &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, + &collisionShapeA, &collisionShapeB, + &collisionShapePositionAObj,&collisionShapePositionBObj, + &collisionShapeOrientationA,&collisionShapeOrientationBObj, + &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -6025,8 +6040,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py } commandHandle = b3InitClosestDistanceQuery(sm); - b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); - b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB); + if (bodyUniqueIdA>=0) + { + b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); + } + if (bodyUniqueIdB>=0) + { + b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB); + } b3SetClosestDistanceThreshold(commandHandle, distanceThreshold); if (linkIndexA >= -1) { @@ -6036,6 +6057,34 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py { b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB); } + if (collisionShapeA>=0) + { + b3SetClosestDistanceFilterCollisionShapeA(commandHandle, collisionShapeA); + } + if (collisionShapeB>=0) + { + b3SetClosestDistanceFilterCollisionShapeB(commandHandle, collisionShapeB); + } + if (collisionShapePositionAObj) + { + pybullet_internalSetVectord(collisionShapePositionAObj,collisionShapePositionA); + b3SetClosestDistanceFilterCollisionShapePositionA(commandHandle, collisionShapePositionA); + } + if (collisionShapePositionBObj) + { + pybullet_internalSetVectord(collisionShapePositionBObj,collisionShapePositionB); + b3SetClosestDistanceFilterCollisionShapePositionB(commandHandle, collisionShapePositionB); + } + if (collisionShapeOrientationAObj) + { + pybullet_internalSetVector4d(collisionShapeOrientationAObj,collisionShapeOrientationA); + b3SetClosestDistanceFilterCollisionShapeOrientationA(commandHandle, collisionShapeOrientationA); + } + if (collisionShapeOrientationBObj) + { + pybullet_internalSetVector4d(collisionShapeOrientationBObj,collisionShapeOrientationB); + b3SetClosestDistanceFilterCollisionShapeOrientationB(commandHandle, collisionShapeOrientationB); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); From b73b05e9fb9f3b0aba8df04cafbb9021580073e9 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 22 Sep 2018 14:18:21 -0700 Subject: [PATCH 4/4] add getClosestPoints.py example. allow to perform a getClosestPoints query with a collisionShape and world transform (position, orientation) that isn't part of the world. (use createCollisionShape to create it) add optional removeCollisionShape, for collision shapes only used in a query (and not used to create a body) --- examples/SharedMemory/PhysicsClientC_API.cpp | 16 +++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 62 +++++++++++++++++-- examples/SharedMemory/SharedMemoryCommands.h | 2 + .../pybullet/examples/getClosestPoints.py | 31 +++++++--- examples/pybullet/pybullet.c | 37 +++++++++++ 6 files changed, 139 insertions(+), 11 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 8d35be32d..eb22a4896 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2858,6 +2858,22 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClien return (b3SharedMemoryCommandHandle)command; } +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_REMOVE_BODY; + command->m_updateFlags = 0; + command->m_removeObjectArgs.m_numBodies = 0; + command->m_removeObjectArgs.m_numUserConstraints = 0; + command->m_removeObjectArgs.m_numUserCollisionShapes = 1; + command->m_removeObjectArgs.m_userCollisionShapes[0] = collisionShapeId; + return (b3SharedMemoryCommandHandle)command; +} B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 5bdbdaf7b..dfecdf094 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -496,6 +496,8 @@ B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandH B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[/*3*/], double childOrientation[/*4*/]); B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId); + B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient); B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius); B3_SHARED_API int b3CreateVisualShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[/*3*/]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 70c216959..f74630cef 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -232,9 +232,16 @@ struct InternalCollisionShapeData { btCollisionShape* m_collisionShape; b3AlignedObjectArray m_urdfCollisionObjects; + int m_used; + InternalCollisionShapeData() + :m_collisionShape(0), + m_used(0) + { + } void clear() { m_collisionShape=0; + m_used = 0; } }; @@ -2383,6 +2390,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId); if (handle && handle->m_collisionShape) { + handle->m_used++; if (handle->m_collisionShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) { btCompoundShape* childCompound = (btCompoundShape*)handle->m_collisionShape; @@ -6270,6 +6278,7 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[3]); } InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeA); + if (handle && handle->m_collisionShape) { colObA.setCollisionShape(handle->m_collisionShape); @@ -6280,6 +6289,9 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand colObA.setWorldTransform(tr); setA.push_back(&colObA); setALinkIndex.push_back(-2); + } else + { + b3Warning("collisionShapeA provided is not valid."); } } if (collisionShapeB>=0) @@ -6311,6 +6323,9 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand colObB.setWorldTransform(tr); setB.push_back(&colObB); setBLinkIndex.push_back(-2); + } else + { + b3Warning("collisionShapeB provided is not valid."); } } @@ -6436,11 +6451,9 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand m_cachedContactPoints.push_back(pt); } return 1; - } }; - MyContactResultCallback cb(m_data->m_cachedContactPoints); cb.m_bodyUniqueIdA = bodyUniqueIdA; @@ -8663,9 +8676,50 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared } m_data->m_bodyHandles.freeHandle(bodyUniqueId); } - - } + for (int i=0;im_userCollisionShapeHandles.getHandle(removeCollisionShapeId); + if (handle && handle->m_collisionShape) + { + if (handle->m_used) + { + b3Warning("Don't remove collision shape: it is used."); + } + else + { + b3Warning("TODO: dealloc"); + int foundIndex = -1; + + for (int i=0;im_worldImporters.size();i++) + { + btMultiBodyWorldImporter* importer = m_data->m_worldImporters[i]; + for (int c=0;cgetNumCollisionShapes();c++) + { + if (importer->getCollisionShapeByIndex(c) == handle->m_collisionShape) + { + if ( (importer->getNumRigidBodies()==0)&& + (importer->getNumConstraints()==0)) + { + foundIndex=i; + break; + } + } + } + } + if (foundIndex>=0) + { + btMultiBodyWorldImporter* importer = m_data->m_worldImporters[i]; + m_data->m_worldImporters.removeAtIndex(foundIndex); + delete importer; + m_data->m_userCollisionShapeHandles.freeHandle(removeCollisionShapeId); + } + } + } + } + + m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1); for (int i=0;i=3.1415*2.): pitch=0 + yaw+= 0.01 + if (yaw>=3.1415*2.): + yaw=0 - baseOrientationB = p.getQuaternionFromEuler([0,pitch,0])#[0,0.5,0.5,0] - p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB) + baseOrientationB = p.getQuaternionFromEuler([yaw,pitch,0]) + if (obB>=0): + p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB) if (useCollisionShapeQuery): pts = p.getClosestPoints(bodyA=-1, bodyB=-1, distance=100, collisionShapeA=geom,collisionShapeB=geomBox, collisionShapePositionA=[0.5,0,1],collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB) + #pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB) else: pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100) @@ -34,5 +45,11 @@ while (p.isConnected()): ptA = pts[0][5] ptB = pts[0][6] p.addUserDebugLine(lineFromXYZ=ptA,lineToXYZ=ptB,lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0,replaceItemUniqueId=lineId); - time.sleep(1./240.) - \ No newline at end of file + #time.sleep(1./240.) + + +#removeCollisionShape is optional: +#only use removeCollisionShape if the collision shape is not used to create a body +#and if you want to keep on creating new collision shapes for different queries (not recommended) +p.removeCollisionShape(geom) +p.removeCollisionShape(geomBox) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index a5c5bb6f6..ba5e479c7 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3099,6 +3099,40 @@ static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObje } } +static PyObject* pybullet_removeCollisionShape(PyObject* self, PyObject* args, PyObject* keywds) +{ + { + int collisionShapeId= -1; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = {"collisionShapeId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &collisionShapeId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (collisionShapeId>=0) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + if (b3CanSubmitCommand(sm)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus( sm, b3InitRemoveCollisionShapeCommand(sm,collisionShapeId)); + statusType = b3GetStatusType(statusHandle); + } + } + } + + Py_INCREF(Py_None); + return Py_None; +} + static PyObject* pybullet_removeBody(PyObject* self, PyObject* args, PyObject* keywds) { { @@ -9293,6 +9327,9 @@ static PyMethodDef SpamMethods[] = { { "createCollisionShapeArray", (PyCFunction)pybullet_createCollisionShapeArray, METH_VARARGS | METH_KEYWORDS, "Create collision shapes. Returns a non-negative (int) unique id, if successfull, negative otherwise." }, + {"removeCollisionShape", (PyCFunction)pybullet_removeCollisionShape, METH_VARARGS | METH_KEYWORDS, + "Remove a collision shape. Only useful when the collision shape is not used in a body (to perform a getClosestPoint query)."}, + {"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS, "Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},