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

implement getJointInfo for objects loaded through SDF

Browse Source
This commit is contained in:
Erwin Coumans
2016-06-15 18:01:41 -07:00
parent 456c844a6b
commit 95a09bec99
10 changed files with 122 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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)
{