From 6c71f37da14e34f7765f07e6833cb81c9ae8e616 Mon Sep 17 00:00:00 2001 From: T4Larson Date: Wed, 5 Aug 2015 21:06:13 +0200 Subject: [PATCH 1/8] fix: btQuaternion.slerp() may result in NaN-quaternions --- src/LinearMath/btQuaternion.h | 38 +++++++++++++++++++---------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index ede769384..7755b4eed 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -533,25 +533,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; - if (btFabs(product) < btScalar(1)) + const btScalar magnitude = btSqrt(length2() * q.length2()); + 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 - const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1); - - const btScalar theta = btAcos(sign * product); - const btScalar s1 = btSin(sign * t * theta); - const btScalar d = btScalar(1.0) / btSin(theta); - const btScalar s0 = btSin((btScalar(1.0) - t) * theta); - - return btQuaternion( - (m_floats[0] * s0 + q.x() * s1) * d, - (m_floats[1] * s0 + q.y() * s1) * d, - (m_floats[2] * s0 + q.z() * s1) * d, - (m_floats[3] * s0 + q.m_floats[3] * s1) * d); + // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + const btScalar theta = btAcos(absproduct); + const btScalar d = btSin(theta); + btAssert(d > btScalar(0)) + + const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1); + const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d; + const btScalar s1 = btSin(sign * t * theta) / d; + + return btQuaternion( + (m_floats[0] * s0 + q.x() * s1), + (m_floats[1] * s0 + q.y() * s1), + (m_floats[2] * s0 + q.z() * s1), + (m_floats[3] * s0 + q.w() * s1)); } else { From d5dcb341888a1d406cfe138a5b09ddf181084d7b Mon Sep 17 00:00:00 2001 From: Michael Mc Donnell Date: Thu, 25 Aug 2016 21:40:55 -0700 Subject: [PATCH 2/8] [Bullet3OpenCL] Fix wrong parantheses around b3Fabs The absolute value of m_appliedImpulse was supposed to be compared to the breakingThreshold. Instead a boolean was created from the comparison, and the absolute value of that was taken (which makes no sense). --- src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp index 4d14bc428..ad477e25d 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp @@ -1071,7 +1071,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArraym_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; From 4636efae2bc8ae80cb20b88b0f6bc76d2fed1f6f Mon Sep 17 00:00:00 2001 From: Michael Mc Donnell Date: Thu, 25 Aug 2016 21:50:45 -0700 Subject: [PATCH 3/8] [Bullet3OpenCL] Fix check for null-terminated string The code essentially dead because it was checking the pointer to an array for null. The check will alway return true. Instead the code was meant to check if the string had anything in it after the call to clGetDeviceInfo. --- src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp b/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp index 369f1d750..0287e0e5d 100644 --- a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp +++ b/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); } From 3b779ad3e94e2c70fbeef27c2cf43648759772c5 Mon Sep 17 00:00:00 2001 From: Logan Su Date: Fri, 23 Sep 2016 11:45:46 -0700 Subject: [PATCH 4/8] Do not add limits for continuous joints. --- examples/Importers/ImportURDFDemo/URDF2Bullet.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 5478b5b7d..1350076e6 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/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) - { - //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); - - btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); - world1->addMultiBodyConstraint(con); - } - + if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) { + //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); + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); + world1->addMultiBodyConstraint(con); + } } else { From 36f7f448809fe785c0acbf714de5bdf3f9374aed Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Fri, 23 Sep 2016 15:57:35 -0700 Subject: [PATCH 5/8] expose b3GetLinkState in pybullet --- examples/pybullet/pybullet.c | 103 ++++++++++++++++++++++++++++++++++- 1 file changed, 102 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 9e82744fa..de455ba49 100644 --- a/examples/pybullet/pybullet.c +++ b/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."}, From fe6c8775da6a07b56297311c32cc14bb8c9ed771 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 23 Sep 2016 21:59:38 -0700 Subject: [PATCH 6/8] fix uninitialized variable in btMultiBody use btAssert to detect 0 determinant in btMatrix3x3 inverse Remove obsolete comment in PosixSharedMemory, the Windows shared memory implementation was done over a year ago --- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 2 +- examples/SharedMemory/PosixSharedMemory.cpp | 2 +- .../Featherstone/btMultiBody.cpp | 23 ++++++++++++++++++- src/BulletDynamics/Featherstone/btMultiBody.h | 1 + src/LinearMath/btMatrix3x3.h | 3 ++- 5 files changed, 27 insertions(+), 4 deletions(-) diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index a7254f889..f7980109a 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/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 ; diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp index b4ecad863..0297d43e2 100644 --- a/examples/SharedMemory/PosixSharedMemory.cpp +++ b/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 diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 76903be98..29b851611 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/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(); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 2b387df1d..82bfbf30d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/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; diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 963c5db97..40cd1e086 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/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, From c979f147e6c3767d61fc0e57b10706b90a0b523f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 25 Sep 2016 15:12:19 -0700 Subject: [PATCH 7/8] Update btQuaternion.h add missing ; after btAssert --- src/LinearMath/btQuaternion.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 70d569fbc..32f0f85d2 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -544,7 +544,7 @@ public: // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp const btScalar theta = btAcos(absproduct); const btScalar d = btSin(theta); - btAssert(d > btScalar(0)) + btAssert(d > btScalar(0)); const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1); const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d; From 994a797af70d49cf66992c8e4224e7e87d9bb16d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 25 Sep 2016 18:30:54 -0700 Subject: [PATCH 8/8] Update AUTHORS.txt add Yunfei Bai (contributions in various areas, including btMultiBody constraints, inverse kinematics and other areas) --- AUTHORS.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/AUTHORS.txt b/AUTHORS.txt index 8e5b831e7..556e6f641 100644 --- a/AUTHORS.txt +++ b/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