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 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 { 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/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."}, 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); } 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; 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, diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index f7dafcc83..32f0f85d2 100644 --- a/src/LinearMath/btQuaternion.h +++ b/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; - 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 {