Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
YunfeiBai
2016-09-26 12:44:19 -07:00
parent 539fe505a4 994a797af7
commit 2837958bca
11 changed files with 159 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
AUTHORS.txt
View File

@@ -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

15
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -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
{

2
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

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

2
examples/SharedMemory/PosixSharedMemory.cpp
View File

@@ -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

103
examples/pybullet/pybullet.c
View File

@@ -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."},

2
src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp
View File

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

2
src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp
View File

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

23
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

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

1
src/BulletDynamics/Featherstone/btMultiBody.h
View File

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

3
src/LinearMath/btMatrix3x3.h
View File

@@ -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,

38
src/LinearMath/btQuaternion.h
View File

@@ -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
{