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

Allow batches of debug lines. The PhysicsClientExample rendering is still slow (one line at a time, instead of batches)

Browse Source 
Prepare for IMU sensor in Shared Memory Server
This commit is contained in:
erwin coumans
2015-09-03 14:18:22 -07:00
parent 5a0ca58436
commit f75df90d82
9 changed files with 203 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -432,23 +432,23 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
} }
int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines;
btScalar* linesFrom = (btScalar*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; float* linesFrom = (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0];
btScalar* linesTo = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3)); float* linesTo = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*3*sizeof(float));
btScalar* linesColor = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3)); float* linesColor = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*3*sizeof(float));
m_data->m_debugLinesFrom.resize(numLines); m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines);
m_data->m_debugLinesTo.resize(numLines); m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines);
m_data->m_debugLinesColor.resize(numLines); m_data->m_debugLinesColor.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines);
for (int i=0;i<numLines;i++) for (int i=0;i<numLines;i++)
{ {
btVector3 from(linesFrom[i*4],linesFrom[i*4+1],linesFrom[i*4+2]); btVector3 from(linesFrom[i*3],linesFrom[i*3+1],linesFrom[i*3+2]);
btVector3 to(linesTo[i*4],linesTo[i*4+1],linesTo[i*4+2]); btVector3 to(linesTo[i*3],linesTo[i*3+1],linesTo[i*3+2]);
btVector3 color(linesColor[i*4],linesColor[i*4+1],linesColor[i*4+2]); btVector3 color(linesColor[i*3],linesColor[i*3+1],linesColor[i*3+2]);
m_data->m_debugLinesFrom[i] = from; m_data->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = from;
m_data->m_debugLinesTo[i] = to; m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = to;
m_data->m_debugLinesColor[i] = color; m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = color;
} }
break; break;
} }

13
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -193,6 +193,7 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_SENSOR); b3Assert(command->m_type == CMD_CREATE_SENSOR);
int curIndex = command->m_createSensorArguments.m_numJointSensorChanges; 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_jointIndex[curIndex] = jointIndex;
command->m_createSensorArguments.m_enableJointForceSensor[curIndex] = enable; command->m_createSensorArguments.m_enableJointForceSensor[curIndex] = enable;
@@ -200,6 +201,18 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c
return 0; 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() b3PhysicsClientHandle b3ConnectSharedMemory()
{ {

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -65,7 +65,7 @@ int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double
int b3CreateSensorCommandInit(struct SharedMemoryCommand* command); int b3CreateSensorCommandInit(struct SharedMemoryCommand* command);
int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int dofIndex, int enable); int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int dofIndex, int enable);
int b3CreateSensorEnableIMUForLink(struct SharedMemoryCommand* command, int linkIndex, int enable);
int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command); int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command);

58
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -74,11 +74,44 @@ public:
const btVector3* fromLines = m_physicsClient.getDebugLinesFrom(); const btVector3* fromLines = m_physicsClient.getDebugLinesFrom();
const btVector3* toLines = m_physicsClient.getDebugLinesTo(); const btVector3* toLines = m_physicsClient.getDebugLinesTo();
const btVector3* colorLines = m_physicsClient.getDebugLinesColor(); const btVector3* colorLines = m_physicsClient.getDebugLinesColor();
btScalar lineWidth = 2;
int lineWidth = 1;
if (1)
{
btAlignedObjectArray<btVector3FloatData> points;
points.resize(numLines*2);
btAlignedObjectArray<unsigned int> indices;
indices.resize(numLines*2);
for (int i=0;i<numLines;i++) for (int i=0;i<numLines;i++)
{ {
this->m_guiHelper->getRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth); points[i*2].m_floats[0] = fromLines[i].x();
points[i*2].m_floats[1] = fromLines[i].y();
points[i*2].m_floats[2] = fromLines[i].z();
points[i*2+1].m_floats[0] = toLines[i].x();
points[i*2+1].m_floats[1] = toLines[i].y();
points[i*2+1].m_floats[2] = toLines[i].z();
indices[i*2] = i*2;
indices[i*2+1] = i*2+1;
} }
float color[4] = {1,1,0,1};
if (points.size() && indices.size())
{
m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0],color,points.size(),sizeof(btVector3FloatData),&indices[0],indices.size(),lineWidth);
}
} else
{
for (int i=0;i<numLines;i++)
{
m_guiHelper->getRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth);
}
}
} }
virtual void physicsDebugDraw(int debugFlags){} virtual void physicsDebugDraw(int debugFlags){}
virtual bool mouseMoveCallback(float x,float y){return false;}; 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: case CMD_LOAD_URDF:
{ {
command.m_type =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_urdfArguments.m_initialPosition[0] = 0.0;
command.m_updateFlags = command.m_updateFlags =
URDF_ARGS_FILE_NAME| URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION|URDF_ARGS_USE_MULTIBODY|URDF_ARGS_USE_FIXED_BASE; 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; command.m_type =CMD_STEP_FORWARD_SIMULATION;
cl->enqueueCommand(command); cl->enqueueCommand(command);
command.m_type =CMD_REQUEST_DEBUG_LINES; 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); cl->enqueueCommand(command);
break; 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); 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) if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{ {
for (int i=0;i<m_physicsClient.getNumJoints();i++) for (int i=0;i<m_physicsClient.getNumJoints();i++)
@@ -330,6 +360,18 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
} }
} }
if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED)
{
SharedMemoryCommand command;
if (status.m_sendDebugLinesArgs.m_numRemainingDebugLines>0)
{
//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_physicsClient.canSubmitCommand())
{ {
if (m_userCommandRequests.size()) if (m_userCommandRequests.size())

104
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -14,6 +14,7 @@
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/btTransform.h"
#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
@@ -37,7 +38,7 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
{ {
int m_debugMode; int m_debugMode;
btAlignedObjectArray<SharedMemLines> m_lines; btAlignedObjectArray<SharedMemLines> m_lines2;
SharedMemoryDebugDrawer () SharedMemoryDebugDrawer ()
:m_debugMode(0) :m_debugMode(0)
@@ -70,7 +71,7 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
line.m_from = from; line.m_from = from;
line.m_to = to; line.m_to = to;
line.m_color = color; line.m_color = color;
m_lines.push_back(line); m_lines2.push_back(line);
} }
}; };
struct PhysicsServerInternalData struct PhysicsServerInternalData
@@ -94,6 +95,9 @@ struct PhysicsServerInternalData
btMultiBodyDynamicsWorld* m_dynamicsWorld; btMultiBodyDynamicsWorld* m_dynamicsWorld;
SharedMemoryDebugDrawer* m_debugDrawer; SharedMemoryDebugDrawer* m_debugDrawer;
btTransform m_rootLocalInertialFrame;
struct GUIHelperInterface* m_guiHelper; struct GUIHelperInterface* m_guiHelper;
int m_sharedMemoryKey; int m_sharedMemoryKey;
@@ -110,6 +114,7 @@ struct PhysicsServerInternalData
m_sharedMemoryKey(SHARED_MEMORY_KEY), m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_verboseOutput(false) m_verboseOutput(false)
{ {
m_rootLocalInertialFrame.setIdentity();
} }
SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp) 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); BulletURDFImporter u2b(m_data->m_guiHelper);
bool loadOk = u2b.loadURDF(fileName, useFixedBase); bool loadOk = u2b.loadURDF(fileName, useFixedBase);
if (loadOk) 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) if (m_data->m_verboseOutput)
{ {
b3Printf("loaded %s OK!", fileName); b3Printf("loaded %s OK!", fileName);
@@ -548,45 +561,64 @@ void PhysicsServerSharedMemory::processClientCommands()
case CMD_REQUEST_DEBUG_LINES: case CMD_REQUEST_DEBUG_LINES:
{ {
int curFlags =m_data->m_debugDrawer->getDebugMode(); int curFlags =m_data->m_debugDrawer->getDebugMode();
int debugMode = btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb;
m_data->m_debugDrawer->m_lines.resize(0); int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb;
int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex;
if (startingLineIndex<0)
{
b3Warning("startingLineIndex should be non-negative");
startingLineIndex = 0;
}
if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0)
{
m_data->m_debugDrawer->m_lines2.resize(0);
//|btIDebugDraw::DBG_DrawAabb| //|btIDebugDraw::DBG_DrawAabb|
// btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ;
m_data->m_debugDrawer->setDebugMode(debugMode); m_data->m_debugDrawer->setDebugMode(debugMode);
m_data->m_dynamicsWorld->debugDrawWorld(); m_data->m_dynamicsWorld->debugDrawWorld();
m_data->m_debugDrawer->setDebugMode(curFlags); m_data->m_debugDrawer->setDebugMode(curFlags);
}
int numLines = m_data->m_debugDrawer->m_lines.size(); //9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color'
int memRequirements = numLines*sizeof(btVector3)*3; int maxNumLines = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE/(sizeof(float)*9)-1;
if (memRequirements<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) 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)
{ {
btScalar* linesFrom = (btScalar*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; float* linesFrom = (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0];
btScalar* linesTo = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3)); float* linesTo = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*3*sizeof(float));
btScalar* linesColor = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3)); float* linesColor = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*3*sizeof(float));
for (int i=0;i<numLines;i++) for (int i=0;i<numLines;i++)
{ {
linesFrom[i*4] = m_data->m_debugDrawer->m_lines[i].m_from.x(); linesFrom[i*3] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_from.x();
linesTo[i*4] = m_data->m_debugDrawer->m_lines[i].m_to.x(); linesTo[i*3] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_to.x();
linesColor[i*4] = m_data->m_debugDrawer->m_lines[i].m_color.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(); linesFrom[i*3+1] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_from.y();
linesTo[i*4+1] = m_data->m_debugDrawer->m_lines[i].m_to.y(); linesTo[i*3+1] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_to.y();
linesColor[i*4+1] = m_data->m_debugDrawer->m_lines[i].m_color.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(); linesFrom[i*3+2] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_from.z();
linesTo[i*4+2] = m_data->m_debugDrawer->m_lines[i].m_to.z(); linesTo[i*3+2] = m_data->m_debugDrawer->m_lines2[i+startingLineIndex].m_to.z();
linesColor[i*4+2] = m_data->m_debugDrawer->m_lines[i].m_color.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); SharedMemoryStatus& status = m_data->createServerStatus(CMD_DEBUG_LINES_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
status.m_sendDebugLinesArgs.m_numDebugLines = numLines; 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); m_data->submitServerStatus(status);
} else
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_DEBUG_LINES_OVERFLOW_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
}
break; break;
} }
case CMD_LOAD_URDF: case CMD_LOAD_URDF:
@@ -888,6 +920,24 @@ void PhysicsServerSharedMemory::processClientCommands()
tr.setOrigin(mb->getBasePos()); tr.setOrigin(mb->getBasePos());
tr.setRotation(mb->getWorldToBaseRot().inverse()); 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 //base position in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
@@ -1098,13 +1148,15 @@ void PhysicsServerSharedMemory::renderScene()
void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags) void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)
{ {
#if 0
if (m_data->m_dynamicsWorld) if (m_data->m_dynamicsWorld)
{ {
if (m_data->m_dynamicsWorld->getDebugDrawer()) if (m_data->m_dynamicsWorld->getDebugDrawer())
{ {
m_data->m_debugDrawer->m_lines.clear(); //m_data->m_debugDrawer->m_lines.clear();
m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); //m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
} }
m_data->m_dynamicsWorld->debugDrawWorld(); m_data->m_dynamicsWorld->debugDrawWorld();
} }
#endif
} }

2
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -18,6 +18,7 @@ class PhysicsServerExample : public SharedMemoryCommon
bool m_wantsShutdown; bool m_wantsShutdown;
bool m_isConnected; bool m_isConnected;
btClock m_clock;
public: public:
@@ -111,6 +112,7 @@ bool PhysicsServerExample::wantsTermination()
void PhysicsServerExample::stepSimulation(float deltaTime) void PhysicsServerExample::stepSimulation(float deltaTime)
{ {
m_physicsServer.processClientCommands(); m_physicsServer.processClientCommands();
} }

3
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -133,7 +133,7 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
{ {
command.m_type =CMD_LOAD_URDF; command.m_type =CMD_LOAD_URDF;
command.m_updateFlags = URDF_ARGS_FILE_NAME|URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION; 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[0] = 0.0;
command.m_urdfArguments.m_initialPosition[1] = 0.0; command.m_urdfArguments.m_initialPosition[1] = 0.0;
command.m_urdfArguments.m_initialPosition[2] = 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_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex; motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_jointIndex = jointIndex; 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_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex;
sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true; sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true;
sensorCommand.m_createSensorArguments.m_numJointSensorChanges++; sensorCommand.m_createSensorArguments.m_numJointSensorChanges++;

18
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -130,11 +130,14 @@ struct InitPoseArgs
struct RequestDebugLinesArgs struct RequestDebugLinesArgs
{ {
int m_debugMode; int m_debugMode;
int m_startingLineIndex;
}; };
struct SendDebugLinesArgs struct SendDebugLinesArgs
{ {
int m_startingLineIndex;
int m_numDebugLines; int m_numDebugLines;
int m_numRemainingDebugLines;
}; };
@@ -198,6 +201,8 @@ struct SendActualStateArgs
int m_numDegreeOfFreedomQ; int m_numDegreeOfFreedomQ;
int m_numDegreeOfFreedomU; int m_numDegreeOfFreedomU;
double m_rootLocalInertialFrame[7];
//actual state is only written by the server, read-only access by client is expected //actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
double m_actualStateQdot[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 struct CreateSensorArgs
{ {
int m_bodyUniqueId; int m_bodyUniqueId;
int m_numJointSensorChanges; 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_jointIndex[MAX_DEGREE_OF_FREEDOM];
int m_enableJointForceSensor[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; typedef struct SharedMemoryCommand SharedMemoryCommand_t;

14
test/SharedMemory/test.c
View File

@@ -54,13 +54,21 @@ int main(int argc, char* argv[])
timeout = MAX_TIMEOUT; timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
b3Printf("timeout = %d\n",timeout); b3Printf("timeout = %d\n",timeout);
int imuLinkIndex = -1;
numJoints = b3GetNumJoints(sm); numJoints = b3GetNumJoints(sm);
for (i=0;i<numJoints;i++) for (i=0;i<numJoints;i++)
{ {
struct b3JointInfo jointInfo; struct b3JointInfo jointInfo;
b3GetJointInfo(sm,i,&jointInfo); b3GetJointInfo(sm,i,&jointInfo);
printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName); printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
//pick the IMU link index based on torso name
if (strstr(jointInfo.m_linkName,"base_link"))
{
imuLinkIndex = i;
}
//pick the joint index based on joint name //pick the joint index based on joint name
if (strstr(jointInfo.m_jointName,"base_to_left_leg")) if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
{ {
@@ -76,6 +84,12 @@ int main(int argc, char* argv[])
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0)) if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{ {
ret = b3CreateSensorCommandInit(&command); ret = b3CreateSensorCommandInit(&command);
if (imuLinkIndex>=0)
{
ret = b3CreateSensorEnableIMUForLink(&command, imuLinkIndex, 1);
}
if (sensorJointIndexLeft>=0) if (sensorJointIndexLeft>=0)
{ {
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1); ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1);