pybullet.getAABB and getAPIVersion

fix btMultiBody::getLinkCollider
bump up Bullet C-API version

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,

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);
 

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;
 

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

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

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

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<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.
-	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
 
     
     //