From f75df90d826736faa96944e204ddd4b093b5f475 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 3 Sep 2015 14:18:22 -0700 Subject: [PATCH] Allow batches of debug lines. The PhysicsClientExample rendering is still slow (one line at a time, instead of batches) Prepare for IMU sensor in Shared Memory Server --- examples/SharedMemory/PhysicsClient.cpp | 24 ++-- examples/SharedMemory/PhysicsClientC_API.cpp | 13 ++ examples/SharedMemory/PhysicsClientC_API.h | 4 +- .../SharedMemory/PhysicsClientExample.cpp | 62 +++++++-- examples/SharedMemory/PhysicsServer.cpp | 124 +++++++++++++----- .../SharedMemory/PhysicsServerExample.cpp | 2 + examples/SharedMemory/RobotControlExample.cpp | 3 +- examples/SharedMemory/SharedMemoryCommands.h | 18 +++ test/SharedMemory/test.c | 14 ++ 9 files changed, 203 insertions(+), 61 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index e33d1dbc6..413c87a26 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -432,23 +432,23 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; - btScalar* linesFrom = (btScalar*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; - btScalar* linesTo = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3)); - btScalar* linesColor = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3)); + float* linesFrom = (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; + float* linesTo = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*3*sizeof(float)); + float* linesColor = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*3*sizeof(float)); - m_data->m_debugLinesFrom.resize(numLines); - m_data->m_debugLinesTo.resize(numLines); - m_data->m_debugLinesColor.resize(numLines); + m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines); + m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines); + m_data->m_debugLinesColor.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines); for (int i=0;im_debugLinesFrom[i] = from; - m_data->m_debugLinesTo[i] = to; - m_data->m_debugLinesColor[i] = color; + m_data->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = from; + m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = to; + m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = color; } break; } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 47a977bd9..08b1df5dc 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -193,6 +193,7 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c b3Assert(command); b3Assert(command->m_type == CMD_CREATE_SENSOR); int curIndex = command->m_createSensorArguments.m_numJointSensorChanges; + command->m_createSensorArguments.m_sensorType[curIndex] = SENSOR_FORCE_TORQUE; command->m_createSensorArguments.m_jointIndex[curIndex] = jointIndex; command->m_createSensorArguments.m_enableJointForceSensor[curIndex] = enable; @@ -200,6 +201,18 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c return 0; } +int b3CreateSensorEnableIMUForLink(struct SharedMemoryCommand* command, int linkIndex, int enable) +{ + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_SENSOR); + int curIndex = command->m_createSensorArguments.m_numJointSensorChanges; + command->m_createSensorArguments.m_sensorType[curIndex] = SENSOR_IMU; + command->m_createSensorArguments.m_linkIndex[curIndex] = linkIndex; + command->m_createSensorArguments.m_enableSensor[curIndex] = enable; + command->m_createSensorArguments.m_numJointSensorChanges++; + return 0; +} + b3PhysicsClientHandle b3ConnectSharedMemory() { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index cc3c6d420..04edf21b9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -63,9 +63,9 @@ int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, d int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double halfExtentsX,double halfExtentsY,double halfExtentsZ); - int b3CreateSensorCommandInit(struct SharedMemoryCommand* command); +int b3CreateSensorCommandInit(struct SharedMemoryCommand* command); int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int dofIndex, int enable); - +int b3CreateSensorEnableIMUForLink(struct SharedMemoryCommand* command, int linkIndex, int enable); int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index ca83b90c0..0cb31279d 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -74,11 +74,44 @@ public: const btVector3* fromLines = m_physicsClient.getDebugLinesFrom(); const btVector3* toLines = m_physicsClient.getDebugLinesTo(); const btVector3* colorLines = m_physicsClient.getDebugLinesColor(); - btScalar lineWidth = 2; - for (int i=0;im_guiHelper->getRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth); + btAlignedObjectArray points; + points.resize(numLines*2); + btAlignedObjectArray indices; + indices.resize(numLines*2); + + for (int i=0;igetRenderInterface()->drawLines(&points[0].m_floats[0],color,points.size(),sizeof(btVector3FloatData),&indices[0],indices.size(),lineWidth); + } + } else + { + for (int i=0;igetRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth); + } } + } virtual void physicsDebugDraw(int debugFlags){} virtual bool mouseMoveCallback(float x,float y){return false;}; @@ -107,7 +140,7 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) case CMD_LOAD_URDF: { command.m_type =CMD_LOAD_URDF; - sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf"); + sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf"); command.m_urdfArguments.m_initialPosition[0] = 0.0; command.m_updateFlags = URDF_ARGS_FILE_NAME| URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION|URDF_ARGS_USE_MULTIBODY|URDF_ARGS_USE_FIXED_BASE; @@ -145,6 +178,8 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) command.m_type =CMD_STEP_FORWARD_SIMULATION; cl->enqueueCommand(command); command.m_type =CMD_REQUEST_DEBUG_LINES; + command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//:DBG_DrawConstraints; + command.m_requestDebugLinesArguments.m_startingLineIndex = 0; cl->enqueueCommand(command); break; } @@ -293,12 +328,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } - if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED) - { - //store the debug lines for drawing - - - } + if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { for (int i=0;i0) + { + //continue requesting debug lines for drawing + command.m_type =CMD_REQUEST_DEBUG_LINES; + command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//DBG_DrawConstraints; + command.m_requestDebugLinesArguments.m_startingLineIndex = status.m_sendDebugLinesArgs.m_numDebugLines+status.m_sendDebugLinesArgs.m_startingLineIndex; + enqueueCommand(command); + } + } if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 4b6a0ad08..eb413ceda 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -14,6 +14,7 @@ #include "btBulletDynamicsCommon.h" +#include "LinearMath/btTransform.h" #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" @@ -37,7 +38,7 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw { int m_debugMode; - btAlignedObjectArray m_lines; + btAlignedObjectArray m_lines2; SharedMemoryDebugDrawer () :m_debugMode(0) @@ -70,7 +71,7 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw line.m_from = from; line.m_to = to; line.m_color = color; - m_lines.push_back(line); + m_lines2.push_back(line); } }; struct PhysicsServerInternalData @@ -94,6 +95,9 @@ struct PhysicsServerInternalData btMultiBodyDynamicsWorld* m_dynamicsWorld; SharedMemoryDebugDrawer* m_debugDrawer; + btTransform m_rootLocalInertialFrame; + + struct GUIHelperInterface* m_guiHelper; int m_sharedMemoryKey; @@ -110,6 +114,7 @@ struct PhysicsServerInternalData m_sharedMemoryKey(SHARED_MEMORY_KEY), m_verboseOutput(false) { + m_rootLocalInertialFrame.setIdentity(); } SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp) @@ -415,9 +420,17 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& BulletURDFImporter u2b(m_data->m_guiHelper); + bool loadOk = u2b.loadURDF(fileName, useFixedBase); if (loadOk) { + { + btScalar mass = 0; + m_data->m_rootLocalInertialFrame.setIdentity(); + btVector3 localInertiaDiagonal(0,0,0); + int urdfLinkIndex = u2b.getRootLinkIndex(); + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,m_data->m_rootLocalInertialFrame); + } if (m_data->m_verboseOutput) { b3Printf("loaded %s OK!", fileName); @@ -548,45 +561,64 @@ void PhysicsServerSharedMemory::processClientCommands() case CMD_REQUEST_DEBUG_LINES: { int curFlags =m_data->m_debugDrawer->getDebugMode(); - int debugMode = btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; - m_data->m_debugDrawer->m_lines.resize(0); - //|btIDebugDraw::DBG_DrawAabb| - // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; - m_data->m_debugDrawer->setDebugMode(debugMode); - m_data->m_dynamicsWorld->debugDrawWorld(); - m_data->m_debugDrawer->setDebugMode(curFlags); - - int numLines = m_data->m_debugDrawer->m_lines.size(); - int memRequirements = numLines*sizeof(btVector3)*3; - if (memRequirementsm_debugDrawer->m_lines2.resize(0); + //|btIDebugDraw::DBG_DrawAabb| + // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; + m_data->m_debugDrawer->setDebugMode(debugMode); + m_data->m_dynamicsWorld->debugDrawWorld(); + m_data->m_debugDrawer->setDebugMode(curFlags); + } - btScalar* linesFrom = (btScalar*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; - btScalar* linesTo = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3)); - btScalar* linesColor = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3)); + //9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color' + int maxNumLines = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE/(sizeof(float)*9)-1; + if (startingLineIndex >m_data->m_debugDrawer->m_lines2.size()) + { + b3Warning("m_startingLineIndex exceeds total number of debug lines"); + startingLineIndex =m_data->m_debugDrawer->m_lines2.size(); + } + + int numLines = btMin(maxNumLines,m_data->m_debugDrawer->m_lines2.size()-startingLineIndex); + + if (numLines) + { + + float* linesFrom = (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; + float* linesTo = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*3*sizeof(float)); + float* linesColor = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*3*sizeof(float)); for (int i=0;im_debugDrawer->m_lines[i].m_from.x(); - linesTo[i*4] = m_data->m_debugDrawer->m_lines[i].m_to.x(); - linesColor[i*4] = m_data->m_debugDrawer->m_lines[i].m_color.x(); + linesFrom[i*3] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_from.x(); + linesTo[i*3] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_to.x(); + linesColor[i*3] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_color.x(); - linesFrom[i*4+1] = m_data->m_debugDrawer->m_lines[i].m_from.y(); - linesTo[i*4+1] = m_data->m_debugDrawer->m_lines[i].m_to.y(); - linesColor[i*4+1] = m_data->m_debugDrawer->m_lines[i].m_color.y(); + linesFrom[i*3+1] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_from.y(); + linesTo[i*3+1] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_to.y(); + linesColor[i*3+1] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_color.y(); - linesFrom[i*4+2] = m_data->m_debugDrawer->m_lines[i].m_from.z(); - linesTo[i*4+2] = m_data->m_debugDrawer->m_lines[i].m_to.z(); - linesColor[i*4+2] = m_data->m_debugDrawer->m_lines[i].m_color.z(); -} - SharedMemoryStatus& status = m_data->createServerStatus(CMD_DEBUG_LINES_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - status.m_sendDebugLinesArgs.m_numDebugLines = numLines; - m_data->submitServerStatus(status); - } else - { - SharedMemoryStatus& status = m_data->createServerStatus(CMD_DEBUG_LINES_OVERFLOW_FAILED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(status); + linesFrom[i*3+2] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_from.z(); + linesTo[i*3+2] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_to.z(); + linesColor[i*3+2] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_color.z(); + } } + + SharedMemoryStatus& status = m_data->createServerStatus(CMD_DEBUG_LINES_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + status.m_sendDebugLinesArgs.m_numDebugLines = numLines; + status.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; + status.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_debugDrawer->m_lines2.size()-(startingLineIndex+numLines); + m_data->submitServerStatus(status); + break; } case CMD_LOAD_URDF: @@ -888,6 +920,24 @@ void PhysicsServerSharedMemory::processClientCommands() tr.setOrigin(mb->getBasePos()); tr.setRotation(mb->getWorldToBaseRot().inverse()); + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = + m_data->m_rootLocalInertialFrame.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = + m_data->m_rootLocalInertialFrame.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = + m_data->m_rootLocalInertialFrame.getOrigin()[2]; + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = + m_data->m_rootLocalInertialFrame.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = + m_data->m_rootLocalInertialFrame.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = + m_data->m_rootLocalInertialFrame.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = + m_data->m_rootLocalInertialFrame.getRotation()[3]; + + + //base position in world space, carthesian serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; @@ -1098,13 +1148,15 @@ void PhysicsServerSharedMemory::renderScene() void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags) { +#if 0 if (m_data->m_dynamicsWorld) { if (m_data->m_dynamicsWorld->getDebugDrawer()) { - m_data->m_debugDrawer->m_lines.clear(); - m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); + //m_data->m_debugDrawer->m_lines.clear(); + //m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); } m_data->m_dynamicsWorld->debugDrawWorld(); } +#endif } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 545365513..0b07e7190 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -18,6 +18,7 @@ class PhysicsServerExample : public SharedMemoryCommon bool m_wantsShutdown; bool m_isConnected; + btClock m_clock; public: @@ -111,6 +112,7 @@ bool PhysicsServerExample::wantsTermination() void PhysicsServerExample::stepSimulation(float deltaTime) { + m_physicsServer.processClientCommands(); } diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index 067dc422f..c08ddc3fb 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -133,7 +133,7 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) { command.m_type =CMD_LOAD_URDF; command.m_updateFlags = URDF_ARGS_FILE_NAME|URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION; - sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf");//r2d2.urdf"); + sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf");//r2d2.urdf"); command.m_urdfArguments.m_initialPosition[0] = 0.0; command.m_urdfArguments.m_initialPosition[1] = 0.0; command.m_urdfArguments.m_initialPosition[2] = 0.0; @@ -537,6 +537,7 @@ void RobotControlExample::stepSimulation(float deltaTime) motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_posIndex = info.m_qIndex; motorInfo->m_jointIndex = jointIndex; + sensorCommand.m_createSensorArguments.m_sensorType[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = SENSOR_FORCE_TORQUE; sensorCommand.m_createSensorArguments.m_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex; sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true; sensorCommand.m_createSensorArguments.m_numJointSensorChanges++; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 6f5c6fd3e..746255366 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -130,11 +130,14 @@ struct InitPoseArgs struct RequestDebugLinesArgs { int m_debugMode; + int m_startingLineIndex; }; struct SendDebugLinesArgs { + int m_startingLineIndex; int m_numDebugLines; + int m_numRemainingDebugLines; }; @@ -198,6 +201,8 @@ struct SendActualStateArgs int m_numDegreeOfFreedomQ; int m_numDegreeOfFreedomU; + double m_rootLocalInertialFrame[7]; + //actual state is only written by the server, read-only access by client is expected double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM]; @@ -207,12 +212,25 @@ struct SendActualStateArgs }; +enum EnumSensorTypes +{ + SENSOR_FORCE_TORQUE=1, + SENSOR_IMU=1, +}; + struct CreateSensorArgs { int m_bodyUniqueId; int m_numJointSensorChanges; + int m_sensorType[MAX_DEGREE_OF_FREEDOM]; + +///todo: clean up the duplication, make sure no-one else is using those members directly (use C-API header instead) int m_jointIndex[MAX_DEGREE_OF_FREEDOM]; int m_enableJointForceSensor[MAX_DEGREE_OF_FREEDOM]; + + int m_linkIndex[MAX_DEGREE_OF_FREEDOM]; + int m_enableSensor[MAX_DEGREE_OF_FREEDOM]; + }; typedef struct SharedMemoryCommand SharedMemoryCommand_t; diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 55fd1dc70..4896cbdd9 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -54,13 +54,21 @@ int main(int argc, char* argv[]) timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} b3Printf("timeout = %d\n",timeout); + int imuLinkIndex = -1; numJoints = b3GetNumJoints(sm); for (i=0;i=0) || (sensorJointIndexRight>=0)) { ret = b3CreateSensorCommandInit(&command); + + if (imuLinkIndex>=0) + { + ret = b3CreateSensorEnableIMUForLink(&command, imuLinkIndex, 1); + } + if (sensorJointIndexLeft>=0) { ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1);