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

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