diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e95713c14..712e4b9fe 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1602,6 +1602,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomU = 0; + if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM) + { + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + break; + } //always add the base, even for static (non-moving objects) //so that we can easily move the 'fixed' base when needed diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f7a9812ca..5e2bd41c5 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -27,7 +27,7 @@ #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024) #define SHARED_MEMORY_SERVER_TEST_C -#define MAX_DEGREE_OF_FREEDOM 64 +#define MAX_DEGREE_OF_FREEDOM 128 #define MAX_NUM_SENSORS 256 #define MAX_URDF_FILENAME_LENGTH 1024 #define MAX_SDF_FILENAME_LENGTH 1024 diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index b81ce57ed..3ed2de4b9 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -268,7 +268,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) renderData.m_viewportMatrix = viewport(0,0,width, height); b3AlignedObjectArray& zbuffer = renderData.m_depthBuffer; - int* segmentationMaskBufferPtr = &renderData.m_segmentationMaskBufferPtr->at(0); + int* segmentationMaskBufferPtr = renderData.m_segmentationMaskBufferPtr?&renderData.m_segmentationMaskBufferPtr->at(0):0; TGAImage& frame = renderData.m_rgbColorBuffer; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 915de51f1..dfe4f292d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -552,7 +552,11 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); const int status_type = b3GetStatusType(status_handle); - + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return NULL; + } const double* actualStateQ; // const double* jointReactionForces[]; int i; @@ -848,42 +852,44 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - pyListJointInfo = PyTuple_New(jointInfoSize); - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - - // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", - // info.m_jointIndex, - // info.m_jointName, - // info.m_jointType, - // info.m_qIndex, - // info.m_uIndex); - // printf(" flags=%d jointDamping=%f jointFriction=%f\n", - // info.m_flags, - // info.m_jointDamping, - // info.m_jointFriction); - PyTuple_SetItem(pyListJointInfo, 0, - PyInt_FromLong(info.m_jointIndex)); - PyTuple_SetItem(pyListJointInfo, 1, - PyString_FromString(info.m_jointName)); - PyTuple_SetItem(pyListJointInfo, 2, - PyInt_FromLong(info.m_jointType)); - PyTuple_SetItem(pyListJointInfo, 3, - PyInt_FromLong(info.m_qIndex)); - PyTuple_SetItem(pyListJointInfo, 4, - PyInt_FromLong(info.m_uIndex)); - PyTuple_SetItem(pyListJointInfo, 5, - PyInt_FromLong(info.m_flags)); - PyTuple_SetItem(pyListJointInfo, 6, - PyFloat_FromDouble(info.m_jointDamping)); - PyTuple_SetItem(pyListJointInfo, 7, - PyFloat_FromDouble(info.m_jointFriction)); - return pyListJointInfo; + if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) + { + + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + PyTuple_SetItem(pyListJointInfo, 0, + PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, + PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, + PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, + PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, + PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + return pyListJointInfo; + } + else + { + PyErr_SetString(SpamError, "GetJointInfo failed."); + return NULL; + } } } @@ -934,12 +940,19 @@ pybullet_getJointState(PyObject* self, PyObject* args) { if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { - + 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, "getBasePositionAndOrientation failed."); + return NULL; + } + pyListJointState = PyTuple_New(sensorStateSize); pyListJointForceTorque = PyTuple_New(forceTorqueSize);