From bb8cfe3c9a62260f1301c743302a94e8a6fdfd5a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 15 Jun 2017 19:46:27 -0700 Subject: [PATCH] pybullet.getAABB and getAPIVersion fix btMultiBody::getLinkCollider bump up Bullet C-API version --- examples/SharedMemory/PhysicsClientC_API.cpp | 34 ++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 49 +++++++++ examples/SharedMemory/SharedMemoryCommands.h | 5 + examples/SharedMemory/SharedMemoryPublic.h | 5 +- examples/pybullet/pybullet.c | 104 ++++++++++++++++++ src/BulletDynamics/Featherstone/btMultiBody.h | 5 +- 7 files changed, 200 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 7c02f71c0..1fa562739 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1308,6 +1308,40 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) return bodyId; } +int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + const SendActualStateArgs &args = status->m_sendActualStateArgs; + btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED); + if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + return 0; + + if (linkIndex==-1) + { + aabbMin[0] = args.m_rootWorldAABBMin[0]; + aabbMin[1] = args.m_rootWorldAABBMin[1]; + aabbMin[2] = args.m_rootWorldAABBMin[2]; + + aabbMax[0] = args.m_rootWorldAABBMax[0]; + aabbMax[1] = args.m_rootWorldAABBMax[1]; + aabbMax[2] = args.m_rootWorldAABBMax[2]; + return 1; + } + + if (linkIndex >= 0 && linkIndex < args.m_numLinks) + { + aabbMin[0] = args.m_linkWorldAABBsMin[0]; + aabbMin[1] = args.m_linkWorldAABBsMin[1]; + aabbMin[2] = args.m_linkWorldAABBsMin[2]; + + aabbMax[0] = args.m_linkWorldAABBsMax[0]; + aabbMax[1] = args.m_linkWorldAABBsMax[1]; + aabbMax[2] = args.m_linkWorldAABBsMax[2]; + return 1; + } + + return 0; +} int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index aefdf469e..299cf1974 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -57,6 +57,8 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, const double* actualStateQdot[], const double* jointReactionForces[]); +int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]); + ///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc. b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8c6fbec09..3ed5def06 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4032,6 +4032,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body && body->m_multiBody) { @@ -4040,6 +4041,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks(); + int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomU = 0; @@ -4074,7 +4077,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = body->m_rootLocalInertialFrame.getRotation()[3]; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[0] = 0; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[1] = 0; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[2] = 0; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[0] = -1; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[1] = -1; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[2] = -1; + + + + if (body->m_multiBody->getBaseCollider()) + { + btVector3 aabbMin,aabbMax; + body->m_multiBody->getBaseCollider()->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[0] = aabbMin[0]; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[1] = aabbMin[1]; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[2] = aabbMin[2]; + + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[0] = aabbMax[0]; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[1] = aabbMax[1]; + serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[2] = aabbMax[2]; + } //base position in world space, carthesian serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; @@ -4176,6 +4200,29 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w(); + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+0] = 0; + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+1] = 0; + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+2] = 0; + + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+0] = -1; + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+1] = -1; + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+2] = -1; + + + + if (body->m_multiBody->getLink(l).m_collider) + { + btVector3 aabbMin,aabbMax; + body->m_multiBody->getLinkCollider(l)->getCollisionShape()->getAabb(mb->getLink(l).m_cachedWorldTransform,aabbMin,aabbMax); + + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+0] = aabbMin[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+1] = aabbMin[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+2] = aabbMin[2]; + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+0] = aabbMax[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+1] = aabbMax[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+2] = aabbMax[2]; + } + btVector3 worldLinVel(0,0,0); btVector3 worldAngVel(0,0,0); @@ -4220,6 +4267,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = 0; + int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomU = 0; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index d6f675fa9..f676f7c35 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -427,10 +427,13 @@ struct RequestActualStateArgs struct SendActualStateArgs { int m_bodyUniqueId; + int m_numLinks; int m_numDegreeOfFreedomQ; int m_numDegreeOfFreedomU; double m_rootLocalInertialFrame[7]; + double m_rootWorldAABBMin[3]; + double m_rootWorldAABBMax[3]; //actual state is only written by the server, read-only access by client is expected double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; @@ -444,6 +447,8 @@ struct SendActualStateArgs double m_linkState[7*MAX_NUM_LINKS]; double m_linkWorldVelocities[6*MAX_NUM_LINKS];//linear velocity and angular velocity in world space (x/y/z each). double m_linkLocalInertialFrames[7*MAX_NUM_LINKS]; + double m_linkWorldAABBsMin[3*MAX_NUM_LINKS]; + double m_linkWorldAABBsMax[3*MAX_NUM_LINKS]; }; enum EnumSensorTypes diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index d6cc19a56..04c06317e 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -4,7 +4,8 @@ #define SHARED_MEMORY_KEY 12347 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures ///my convention is year/month/day/rev -#define SHARED_MEMORY_MAGIC_NUMBER 201706001 +#define SHARED_MEMORY_MAGIC_NUMBER 201706015 +//#define SHARED_MEMORY_MAGIC_NUMBER 201706001 //#define SHARED_MEMORY_MAGIC_NUMBER 201703024 @@ -490,6 +491,8 @@ struct b3LinkState double m_worldLinearVelocity[3]; //only valid when ACTUAL_STATE_COMPUTE_LINKVELOCITY is set (b3RequestActualStateCommandComputeLinkVelocity) double m_worldAngularVelocity[3]; //only valid when ACTUAL_STATE_COMPUTE_LINKVELOCITY is set (b3RequestActualStateCommandComputeLinkVelocity) + double m_worldAABBMin[3];//world space bounding minium and maximum box corners. + double m_worldAABBMax[3]; }; //todo: discuss and decide about control mode and combinations diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d87d04efd..d2cab391f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1972,6 +1972,89 @@ static int pybullet_internalGetBasePositionAndOrientation( return 1; } +static PyObject* pybullet_getAABB(PyObject* self, PyObject* args, PyObject* keywds) +{ + + int bodyUniqueId = -1; + int linkIndex = -1; + + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; + + if (bodyUniqueId < 0) + { + PyErr_SetString(SpamError, "getAABB failed; invalid bodyUniqueId"); + return NULL; + } + + if (linkIndex < -1) + { + PyErr_SetString(SpamError, "getAABB failed; invalid linkIndex"); + return NULL; + } + + cmd_handle = + b3RequestActualStateCommandInit(sm, bodyUniqueId); + status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getAABB failed."); + return NULL; + } + + { + PyObject* pyListAabb=0; + PyObject* pyListAabbMin=0; + PyObject* pyListAabbMax=0; + double aabbMin[3]; + double aabbMax[3]; + int i=0; + if (b3GetStatusAABB(status_handle, linkIndex, aabbMin, aabbMax)) + { + pyListAabb = PyTuple_New(2); + pyListAabbMin = PyTuple_New(3); + pyListAabbMax = PyTuple_New(3); + + for (i=0;i<3;i++) + { + PyTuple_SetItem(pyListAabbMin, i, PyFloat_FromDouble(aabbMin[i])); + PyTuple_SetItem(pyListAabbMax, i, PyFloat_FromDouble(aabbMax[i])); + } + + PyTuple_SetItem(pyListAabb, 0, pyListAabbMin); + PyTuple_SetItem(pyListAabb, 1, pyListAabbMax); + + //PyFloat_FromDouble(basePosition[i]); + + return pyListAabb; + } + } + + } + + PyErr_SetString(SpamError, "getAABB failed."); + return NULL; +} + // Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion // values for the base link of your object // Object is retrieved based on body index, which is the order @@ -2365,6 +2448,18 @@ static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyOb #endif } +// Return the number of joints in an object based on +// body index; body index is based on order of sequence +// the object is loaded into simulation +static PyObject* pybullet_getAPIVersion(PyObject* self, PyObject* args, PyObject* keywds) +{ +#if PY_MAJOR_VERSION >= 3 + return PyLong_FromLong(SHARED_MEMORY_MAGIC_NUMBER); +#else + return PyInt_FromLong(SHARED_MEMORY_MAGIC_NUMBER); +#endif +} + // Return the number of joints in an object based on // body index; body index is based on order of sequence // the object is loaded into simulation @@ -6470,6 +6565,10 @@ static PyMethodDef SpamMethods[] = { "Get the world position and orientation of the base of the object. " "(x,y,z) position vector and (x,y,z,w) quaternion orientation."}, + {"getAABB", (PyCFunction)pybullet_getAABB, + METH_VARARGS | METH_KEYWORDS, + "Get the axis aligned bound box min and max coordinates in world space."}, + {"resetBasePositionAndOrientation", (PyCFunction)pybullet_resetBasePositionAndOrientation, METH_VARARGS | METH_KEYWORDS, "Reset the world position and orientation of the base of the object " @@ -6679,6 +6778,11 @@ static PyMethodDef SpamMethods[] = { {"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS, "Set the timeOut in seconds, used for most of the API calls."}, + + {"getAPIVersion", (PyCFunction)pybullet_getAPIVersion, + METH_VARARGS | METH_KEYWORDS, + "Get version of the API. Compatibility exists for connections using the same API version. Make sure both client and server use the same number of bits (32-bit or 64bit)."}, + // todo(erwincoumans) // saveSnapshot // loadSnapshot diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 6dad0afeb..ac5f0993e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -142,9 +142,9 @@ public: btMultiBodyLinkCollider* getLinkCollider(int index) { - if (index >= 0 && index < m_colliders.size()) + if (index >= 0 && index < getNumLinks()) { - return m_colliders[index]; + return getLink(index).m_collider; } return 0; } @@ -659,7 +659,6 @@ private: btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. btAlignedObjectArray m_links; // array of m_links, excluding the base. index from 0 to num_links-1. - btAlignedObjectArray m_colliders; //