Merge remote-tracking branch 'bp/master'

AUTHORS.txt

@@ -35,5 +35,6 @@ Marten Svanfeldt
 Pierre Terdiman
 Steven Thompson
 Tamas Umenhoffer
+Yunfei Bai
 
 If your name is missing, please send an email to erwin.coumans@gmail.com or file an issue at http://github.com/bulletphysics/bullet3

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -320,15 +320,12 @@ void ConvertURDF2BulletInternal(
                         cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
                         cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
-						if (jointLowerLimit <= jointUpperLimit)
+                        if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
-						{
+                          //std::string name = u2b.getLinkName(urdfLinkIndex);
-							//std::string name = u2b.getLinkName(urdfLinkIndex);
+                          //printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
-							//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
+                          btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
-
+                          world1->addMultiBodyConstraint(con);
-							btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
+                        }
-							world1->addMultiBodyConstraint(con);
-						}
-
                     } else
                     {
 

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -172,7 +172,7 @@ SimpleOpenGL3App::SimpleOpenGL3App(	const char* title, int width,int height, boo
     
     b3Assert(glGetError() ==GL_NO_ERROR);
 
-	m_instancingRenderer = new GLInstancingRenderer(128*1024,64*1024*1024);
+	m_instancingRenderer = new GLInstancingRenderer(128*1024,128*1024*1024);
     m_primRenderer = new GLPrimitiveRenderer(width,height);
     
     m_renderer = m_instancingRenderer ;

examples/SharedMemory/PosixSharedMemory.cpp

@@ -2,7 +2,7 @@
 #include "Bullet3Common/b3Logging.h"
 #include "LinearMath/btScalar.h" //for btAssert
 
-//haven't implemented shared memory on Windows yet, just Linux and Mac
+//Windows implementation is in Win32SharedMemory.cpp
 #ifndef _WIN32
 #define TEST_SHARED_MEMORY
 #endif//_WIN32

examples/pybullet/pybullet.c

@@ -909,6 +909,15 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
   if (size == 2)  // get body index and joint index
   {
     if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) {
+      if (bodyIndex < 0) {
+        PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex");
+        return NULL;
+      }
+      if (jointIndex < 0) {
+        PyErr_SetString(SpamError, "getJointState failed; invalid jointIndex");
+        return NULL;
+      }
+
       int status_type = 0;
       b3SharedMemoryCommandHandle cmd_handle =
           b3RequestActualStateCommandInit(sm, bodyIndex);
@@ -917,7 +926,7 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
 
       status_type = b3GetStatusType(status_handle);
       if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
-        PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
+        PyErr_SetString(SpamError, "getJointState failed.");
         return NULL;
       }
 
@@ -954,6 +963,93 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
   return Py_None;
 }
 
+static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) {
+  PyObject* pyLinkState;
+  PyObject* pyLinkStateWorldPosition;
+  PyObject* pyLinkStateWorldOrientation;
+  PyObject* pyLinkStateLocalInertialPosition;
+  PyObject* pyLinkStateLocalInertialOrientation;
+
+  struct b3LinkState linkState;
+
+  int bodyIndex = -1;
+  int linkIndex = -1;
+  int i;
+
+  if (0 == sm) {
+    PyErr_SetString(SpamError, "Not connected to physics server.");
+    return NULL;
+  }
+
+  if (PySequence_Size(args) == 2)  // body index and link index
+  {
+    if (PyArg_ParseTuple(args, "ii", &bodyIndex, &linkIndex)) {
+      if (bodyIndex < 0) {
+        PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex");
+        return NULL;
+      }
+      if (linkIndex < 0) {
+        PyErr_SetString(SpamError, "getLinkState failed; invalid jointIndex");
+        return NULL;
+      }
+
+      int status_type = 0;
+      b3SharedMemoryCommandHandle cmd_handle =
+          b3RequestActualStateCommandInit(sm, bodyIndex);
+      b3SharedMemoryStatusHandle status_handle =
+          b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
+
+      status_type = b3GetStatusType(status_handle);
+      if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
+        PyErr_SetString(SpamError, "getLinkState failed.");
+        return NULL;
+      }
+
+      b3GetLinkState(sm, status_handle, linkIndex, &linkState);
+
+      pyLinkStateWorldPosition = PyTuple_New(3);
+      for (i = 0; i < 3; ++i) {
+        PyTuple_SetItem(pyLinkStateWorldPosition, i,
+                        PyFloat_FromDouble(linkState.m_worldPosition[i]));
+      }
+
+      pyLinkStateWorldOrientation = PyTuple_New(4);
+      for (i = 0; i < 4; ++i) {
+        PyTuple_SetItem(pyLinkStateWorldOrientation, i,
+                        PyFloat_FromDouble(linkState.m_worldOrientation[i]));
+      }
+
+      pyLinkStateLocalInertialPosition = PyTuple_New(3);
+      for (i = 0; i < 3; ++i) {
+        PyTuple_SetItem(pyLinkStateLocalInertialPosition, i,
+                        PyFloat_FromDouble(linkState.m_localInertialPosition[i]));
+      }
+
+      pyLinkStateLocalInertialOrientation = PyTuple_New(4);
+      for (i = 0; i < 4; ++i) {
+        PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i,
+                        PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
+      }
+
+      pyLinkState = PyTuple_New(4);
+      PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
+      PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
+      PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
+      PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
+
+      return pyLinkState;
+    }
+  } else {
+    PyErr_SetString(
+        SpamError,
+        "getLinkState expects 2 arguments (objectUniqueId and link index).");
+    return NULL;
+  }
+
+  Py_INCREF(Py_None);
+  return Py_None;
+}
+
 // internal function to set a float matrix[16]
 // used to initialize camera position with
 // a view and projection matrix in renderImage()
@@ -1920,6 +2016,11 @@ static PyMethodDef SpamMethods[] = {
     {"getJointState", pybullet_getJointState, METH_VARARGS,
      "Get the state (position, velocity etc) for a joint on a body."},
 
+    {"getLinkState", pybullet_getLinkState, METH_VARARGS,
+     "Provides extra information such as the Cartesian world coordinates"
+     " center of mass (COM) of the link, relative to the world reference"
+     " frame."},
+
     {"resetJointState", pybullet_resetJointState, METH_VARARGS,
      "Reset the state (position, velocity etc) for a joint on a body "
      "instantaneously, not through physics simulation."},

src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp

@@ -560,7 +560,7 @@ void b3OpenCLUtils_printDeviceInfo(cl_device_id device)
 	b3Printf("\t\t\t\t\t3D_MAX_WIDTH\t %u\n", info.m_image3dMaxWidth);
 	b3Printf("\t\t\t\t\t3D_MAX_HEIGHT\t %u\n", info.m_image3dMaxHeight);
 	b3Printf("\t\t\t\t\t3D_MAX_DEPTH\t %u\n", info.m_image3dMaxDepth);
-	if (info.m_deviceExtensions != 0)
+	if (*info.m_deviceExtensions != 0)
 	{
 		b3Printf("\n  CL_DEVICE_EXTENSIONS:%s\n",info.m_deviceExtensions);
 	}

src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp

@@ -1071,7 +1071,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b
 						int orgConstraintIndex = m_gpuData->m_cpuConstraintRows[rowIndex].m_originalConstraintIndex;
 						float breakingThreshold = m_gpuData->m_cpuConstraints[orgConstraintIndex].m_breakingImpulseThreshold;
 					//	printf("rows[%d].m_appliedImpulse=%f\n",rowIndex,rows[rowIndex].m_appliedImpulse);
-						if (b3Fabs((m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold))
+						if (b3Fabs(m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold)
 						{
 
 							m_gpuData->m_cpuConstraints[orgConstraintIndex].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED;

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -122,10 +122,15 @@ btMultiBody::btMultiBody(int n_links,
 		m_useGlobalVelocities(false),
 		m_internalNeedsJointFeedback(false)
 {
+	m_cachedInertiaTopLeft.setValue(0,0,0,0,0,0,0,0,0);
+	m_cachedInertiaTopRight.setValue(0,0,0,0,0,0,0,0,0);
+	m_cachedInertiaLowerLeft.setValue(0,0,0,0,0,0,0,0,0);
+	m_cachedInertiaLowerRight.setValue(0,0,0,0,0,0,0,0,0);
+	m_cachedInertiaValid=false;
+
 	m_links.resize(n_links);
 	m_matrixBuf.resize(n_links + 1);
 
-
     m_baseForce.setValue(0, 0, 0);
     m_baseTorque.setValue(0, 0, 0);
 }
@@ -1012,6 +1017,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	{
         if (num_links > 0) 
 		{
+			m_cachedInertiaValid = true;
 			m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
 			m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
 			m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
@@ -1215,6 +1221,14 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo
         result[5] = rhs_top[2] / m_baseMass;
     } else 
 	{
+		if (!m_cachedInertiaValid)
+		{
+			for (int i=0;i<6;i++)
+			{
+				result[i] = 0.f;
+			}
+			return;
+		}
 		/// Special routine for calculating the inverse of a spatial inertia matrix
 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
 		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
@@ -1261,6 +1275,13 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
 	{
 		/// Special routine for calculating the inverse of a spatial inertia matrix
 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
+		if (!m_cachedInertiaValid)
+		{
+			result.setLinear(btVector3(0,0,0));
+			result.setAngular(btVector3(0,0,0));
+			result.setVector(btVector3(0,0,0),btVector3(0,0,0));
+			return;
+		}
 		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
 		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
 		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -677,6 +677,7 @@ private:
 	btMatrix3x3 m_cachedInertiaTopRight;
 	btMatrix3x3 m_cachedInertiaLowerLeft;
 	btMatrix3x3 m_cachedInertiaLowerRight;
+	bool m_cachedInertiaValid;
 
     bool m_fixedBase;
 

src/LinearMath/btMatrix3x3.h

@@ -1047,7 +1047,8 @@ btMatrix3x3::inverse() const
 {
 	btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
 	btScalar det = (*this)[0].dot(co);
-	btFullAssert(det != btScalar(0.0));
+	//btFullAssert(det != btScalar(0.0));
+	btAssert(det != btScalar(0.0));
 	btScalar s = btScalar(1.0) / det;
 	return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
 		co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,

src/LinearMath/btQuaternion.h

@@ -532,25 +532,29 @@ public:
    * Slerp interpolates assuming constant velocity.  */
 	btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
 	{
-	  btScalar magnitude = btSqrt(length2() * q.length2()); 
-	  btAssert(magnitude > btScalar(0));
 
-    btScalar product = dot(q) / magnitude;
+		const btScalar magnitude = btSqrt(length2() * q.length2());
-    if (btFabs(product) < btScalar(1))
+		btAssert(magnitude > btScalar(0));
+		
+		const btScalar product = dot(q) / magnitude;
+		const btScalar absproduct = btFabs(product);
+		
+		if(absproduct < btScalar(1.0 - SIMD_EPSILON))
 		{
-      // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+			// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
-      const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
+			const btScalar theta = btAcos(absproduct);
+			const btScalar d = btSin(theta);
+			btAssert(d > btScalar(0));
 			
-      const btScalar theta = btAcos(sign * product);
+			const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
-      const btScalar s1 = btSin(sign * t * theta);   
+			const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d;
-      const btScalar d = btScalar(1.0) / btSin(theta);
+			const btScalar s1 = btSin(sign * t * theta) / d;
-      const btScalar s0 = btSin((btScalar(1.0) - t) * theta);
 			
-      return btQuaternion(
+			return btQuaternion(
-          (m_floats[0] * s0 + q.x() * s1) * d,
+				(m_floats[0] * s0 + q.x() * s1),
-          (m_floats[1] * s0 + q.y() * s1) * d,
+				(m_floats[1] * s0 + q.y() * s1),
-          (m_floats[2] * s0 + q.z() * s1) * d,
+				(m_floats[2] * s0 + q.z() * s1),
-          (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
+				(m_floats[3] * s0 + q.w() * s1));
 		}
 		else
 		{