AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #659 from erwincoumans/master

Browse Source 
implement getJointInfo for objects loaded through SDF (shared memory API)
This commit is contained in:
erwincoumans 2016-06-16 09:03:36 -07:00 committed by GitHub
commit 95feb8f530
15 changed files with 289 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/PhysicsClient.h
View File

@ -26,7 +26,7 @@ public:
virtual int getNumJoints(int bodyIndex) const = 0;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

4
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -653,10 +653,10 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
}
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
cl->getJointInfo(bodyIndex, linkIndex,*info);
return cl->getJointInfo(bodyIndex, linkIndex,*info);
}
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -57,7 +57,7 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h

28
examples/SharedMemory/PhysicsClientExample.cpp
View File

@ -237,7 +237,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
case CMD_LOAD_SDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf");
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
@ -656,6 +656,32 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
if (statusType == CMD_SDF_LOADING_COMPLETED)
{
int bodyIndicesOut[1024];
int bodyCapacity = 1024;
int numBodies = b3GetStatusBodyIndices(status, bodyIndicesOut, bodyCapacity);
if (numBodies > bodyCapacity)
{
b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity);
} else
{
/*
for (int i=0;i<numBodies;i++)
{
int bodyUniqueId = bodyIndicesOut[i];
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
b3Printf("numJoints = %d", numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyUniqueId,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
}
*/
}
//int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
//int bodyIndex = b3GetStatusBodyIndex(status);
/*if (bodyIndex>=0)
{

99
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@ -37,6 +37,9 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
SharedMemoryStatus m_lastServerStatus;
int m_counter;
@ -82,15 +85,16 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
}
void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info = bodyJoints->m_jointInfo[jointIndex];
return true;
}
return false;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
@ -167,6 +171,48 @@ bool PhysicsClientSharedMemory::connect() {
return true;
}
///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo
void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
{
bParse::btBulletFile bf(
&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
int flag = bf.getFlags();
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok()) {
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
}
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
SharedMemoryStatus* stat = 0;
@ -208,6 +254,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) {
b3Printf("Server loading the SDF OK\n");
}
break;
}
case CMD_URDF_LOADING_COMPLETED: {
@ -275,6 +322,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_BODY_INFO_COMPLETED:
{
if (m_data->m_verboseOutput) {
b3Printf("Received body info\n");
}
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
processBodyJointInfo(bodyUniqueId, serverCmd);
break;
}
case CMD_SDF_LOADING_FAILED: {
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the SDF...\n");
@ -504,21 +561,49 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
m_data->m_waitingForServer = true;
}
/*if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
{
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0)
{
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
for (int i=0;i<numBodies;i++)
{
m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
}
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
m_data->m_bodyIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_SDF_INFO;
command.m_updateFlags = SDF_REQUEST_INFO_BODY;
command.m_sdfRequestInfoArgs.m_infoIndex = 0;
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
submitClientCommand(command);
return 0;
}
}
*/
if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
{
//are there any bodies left to be processed?
if (m_data->m_bodyIdsRequestInfo.size())
{
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
m_data->m_bodyIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
submitClientCommand(command);
return 0;
} else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
}
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
{

4
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@ -11,7 +11,9 @@ class PhysicsClientSharedMemory : public PhysicsClient {
protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
@ -34,7 +36,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);

111
examples/SharedMemory/PhysicsDirect.cpp
View File

@ -248,6 +248,47 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
}
void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
{
bParse::btBulletFile bf(
&m_data->m_bulletStreamDataServerToClient[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
int flag = bf.getFlags();
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok()) {
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
}
bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
{
if (command.m_type==CMD_REQUEST_DEBUG_LINES)
@ -296,46 +337,35 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
break;
}
case CMD_SDF_LOADING_COMPLETED:
{
//we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i=0;i<numBodies;i++)
{
int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
SharedMemoryCommand infoRequestCommand;
infoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
infoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
SharedMemoryStatus infoStatus;
bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand,infoStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
if (hasStatus)
{
processBodyJointInfo(bodyUniqueId, infoStatus);
}
}
break;
}
case CMD_URDF_LOADING_COMPLETED:
{
if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0)
{
bParse::btBulletFile bf(
&m_data->m_bulletStreamDataServerToClient[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
int flag = bf.getFlags();
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok()) {
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
processBodyJointInfo(bodyIndex,serverCmd);
}
break;
}
@ -363,14 +393,19 @@ int PhysicsDirect::getNumJoints(int bodyIndex) const
return 0;
}
void PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
info = bodyJoints->m_jointInfo[jointIndex];
if (jointIndex < bodyJoints->m_jointInfo.size())
{
info = bodyJoints->m_jointInfo[jointIndex];
return true;
}
}
return false;
}
///todo: move this out of the

4
examples/SharedMemory/PhysicsDirect.h
View File

@ -21,6 +21,8 @@ protected:
bool processCamera(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:
PhysicsDirect();
@ -46,7 +48,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

4
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@ -79,9 +79,9 @@ int PhysicsLoopBack::getNumJoints(int bodyIndex) const
return m_data->m_physicsClient->getNumJoints(bodyIndex);
}
void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
{
m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
}
///todo: move this out of the

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@ -40,7 +40,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

56
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -762,12 +762,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
createJointMotors(mb);
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
UrdfLinkNameMapUtil* util2 = new UrdfLinkNameMapUtil;
m_data->m_urdfLinkNameMapper.push_back(util2);
util2->m_mb = mb;
util2->m_memSerializer = 0;
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
@ -963,7 +958,44 @@ void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient,
}
}
int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes)
{
int streamSizeInBytes = 0;
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
btMultiBody* mb = bodyHandle->m_multiBody;
if (mb)
{
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
m_data->m_urdfLinkNameMapper.push_back(util);
util->m_mb = mb;
util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
for (int i=0;i<mb->getNumLinks();i++)
{
//disable serialization of the collision objects
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName,mb->getLink(i).m_linkName);
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName,mb->getLink(i).m_jointName);
}
util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
util->m_memSerializer->insertHeader();
int len = mb->calculateSerializeBufferSize();
btChunk* chunk = util->m_memSerializer->allocate(len,1);
const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer);
util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
}
return streamSizeInBytes;
}
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
{
@ -1171,6 +1203,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
case CMD_REQUEST_BODY_INFO:
{
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
//stream info into memory
int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
serverStatusOut.m_dataStreamArguments.m_streamChunkLength = streamSizeInBytes;
hasStatus = true;
break;
}
case CMD_LOAD_SDF:
{
const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;

2
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@ -27,6 +27,8 @@ protected:
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
public:
PhysicsServerCommandProcessor();

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -312,7 +312,7 @@ struct SdfLoadedArgs
struct SdfRequestInfoArgs
{
int m_infoIndex;
int m_bodyUniqueId;
};
enum EnumSdfRequestInfoFlags

4
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -19,7 +19,7 @@ enum EnumSharedMemoryClientCommand
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
CMD_REQUEST_ACTUAL_STATE,
CMD_REQUEST_DEBUG_LINES,
CMD_REQUEST_SDF_INFO,
CMD_REQUEST_BODY_INFO,
CMD_STEP_FORWARD_SIMULATION,
CMD_RESET_SIMULATION,
CMD_PICK_BODY,
@ -55,6 +55,8 @@ enum EnumSharedMemoryServerStatus
CMD_RESET_SIMULATION_COMPLETED,
CMD_CAMERA_IMAGE_COMPLETED,
CMD_CAMERA_IMAGE_FAILED,
CMD_BODY_INFO_COMPLETED,
CMD_BODY_INFO_FAILED,
CMD_INVALID_STATUS,
CMD_MAX_SERVER_COMMANDS
};

29
test/SharedMemory/test.c
View File

@ -35,6 +35,7 @@ void testSharedMemory(b3PhysicsClientHandle sm)
int sensorJointIndexLeft=-1;
int sensorJointIndexRight=-1;
const char* urdfFileName = "r2d2.urdf";
const char* sdfFileName = "kuka_iiwa/model.sdf";
double gravx=0, gravy=0, gravz=-9.8;
double timeStep = 1./60.;
double startPosX, startPosY,startPosZ;
@ -52,7 +53,33 @@ void testSharedMemory(b3PhysicsClientHandle sm)
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int bodyIndicesOut[10];//MAX_SDF_BODIES = 10
int numJoints, numBodies;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED);
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
ASSERT_EQ(numBodies,1);
int bodyUniqueId = bodyIndicesOut[0];
numJoints = b3GetNumJoints(sm,bodyUniqueId);
b3Printf("numJoints: %d\n", numJoints);
for (i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo))
{
b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
}
}
//ASSERT_EQ(numBodies ==1);
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;