From b73b05e9fb9f3b0aba8df04cafbb9021580073e9 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 22 Sep 2018 14:18:21 -0700 Subject: [PATCH] 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."},