mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 21:30:09 +00:00
572 lines
15 KiB
C++
572 lines
15 KiB
C++
#include "PhysicsClient.h"
|
|
#include "PosixSharedMemory.h"
|
|
#include "Win32SharedMemory.h"
|
|
#include "LinearMath/btAlignedObjectArray.h"
|
|
#include "LinearMath/btVector3.h"
|
|
|
|
#include "Bullet3Common/b3Logging.h"
|
|
#include "../Utils/b3ResourcePath.h"
|
|
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
|
|
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
|
|
#include "SharedMemoryBlock.h"
|
|
|
|
|
|
//copied from btMultiBodyLink.h
|
|
enum JointType
|
|
{
|
|
eRevoluteType = 0,
|
|
ePrismaticType = 1,
|
|
};
|
|
|
|
struct PhysicsClientSharedMemoryInternalData
|
|
{
|
|
SharedMemoryInterface* m_sharedMemory;
|
|
SharedMemoryBlock* m_testBlock1;
|
|
|
|
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
|
|
btAlignedObjectArray<b3JointInfo> m_jointInfo;
|
|
btAlignedObjectArray<btVector3> m_debugLinesFrom;
|
|
btAlignedObjectArray<btVector3> m_debugLinesTo;
|
|
btAlignedObjectArray<btVector3> m_debugLinesColor;
|
|
|
|
int m_counter;
|
|
bool m_serverLoadUrdfOK;
|
|
bool m_isConnected;
|
|
bool m_waitingForServer;
|
|
bool m_hasLastServerStatus;
|
|
int m_sharedMemoryKey;
|
|
bool m_verboseOutput;
|
|
|
|
PhysicsClientSharedMemoryInternalData()
|
|
:m_sharedMemory(0),
|
|
m_testBlock1(0),
|
|
m_counter(0),
|
|
m_serverLoadUrdfOK(false),
|
|
m_isConnected(false),
|
|
m_waitingForServer(false),
|
|
m_hasLastServerStatus(false),
|
|
m_sharedMemoryKey(SHARED_MEMORY_KEY),
|
|
m_verboseOutput(false)
|
|
{
|
|
}
|
|
|
|
|
|
|
|
void processServerStatus();
|
|
|
|
bool canSubmitCommand() const;
|
|
|
|
|
|
};
|
|
|
|
|
|
int PhysicsClientSharedMemory::getNumJoints() const
|
|
{
|
|
return m_data->m_jointInfo.size();
|
|
}
|
|
|
|
void PhysicsClientSharedMemory::getJointInfo(int index, b3JointInfo& info) const
|
|
{
|
|
info = m_data->m_jointInfo[index];
|
|
}
|
|
|
|
|
|
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
|
|
|
|
{
|
|
m_data = new PhysicsClientSharedMemoryInternalData;
|
|
|
|
#ifdef _WIN32
|
|
m_data->m_sharedMemory = new Win32SharedMemoryClient();
|
|
#else
|
|
m_data->m_sharedMemory = new PosixSharedMemory();
|
|
#endif
|
|
|
|
}
|
|
|
|
PhysicsClientSharedMemory::~PhysicsClientSharedMemory()
|
|
{
|
|
if (m_data->m_isConnected)
|
|
{
|
|
disconnectSharedMemory();
|
|
}
|
|
delete m_data->m_sharedMemory;
|
|
delete m_data;
|
|
}
|
|
|
|
void PhysicsClientSharedMemory::setSharedMemoryKey(int key)
|
|
{
|
|
m_data->m_sharedMemoryKey = key;
|
|
}
|
|
|
|
void PhysicsClientSharedMemory::disconnectSharedMemory ()
|
|
{
|
|
if (m_data->m_isConnected)
|
|
{
|
|
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
|
|
m_data->m_isConnected = false;
|
|
}
|
|
}
|
|
|
|
bool PhysicsClientSharedMemory::isConnected() const
|
|
{
|
|
return m_data->m_isConnected ;
|
|
}
|
|
|
|
bool PhysicsClientSharedMemory::connect()
|
|
{
|
|
///server always has to create and initialize shared memory
|
|
bool allowCreation = false;
|
|
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation);
|
|
|
|
if (m_data->m_testBlock1)
|
|
{
|
|
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
|
|
{
|
|
b3Error("Error: please start server before client\n");
|
|
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
|
|
m_data->m_testBlock1 = 0;
|
|
return false;
|
|
} else
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Connected to existing shared memory, status OK.\n");
|
|
}
|
|
m_data->m_isConnected = true;
|
|
}
|
|
} else
|
|
{
|
|
b3Error("Cannot connect to shared memory");
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
|
|
bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus)
|
|
{
|
|
bool hasStatus = false;
|
|
|
|
if (!m_data->m_testBlock1)
|
|
{
|
|
serverStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
|
|
return true;
|
|
}
|
|
|
|
if (!m_data->m_waitingForServer)
|
|
{
|
|
serverStatus.m_type = CMD_WAITING_FOR_CLIENT_COMMAND;
|
|
return true;
|
|
}
|
|
|
|
if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands)
|
|
{
|
|
btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1);
|
|
|
|
const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
|
|
hasStatus = true;
|
|
serverStatus = serverCmd;
|
|
EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
|
|
//consume the command
|
|
switch (serverCmd.m_type)
|
|
{
|
|
case CMD_CLIENT_COMMAND_COMPLETED:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Server completed command");
|
|
}
|
|
break;
|
|
}
|
|
case CMD_URDF_LOADING_COMPLETED:
|
|
{
|
|
m_data->m_serverLoadUrdfOK = true;
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Server loading the URDF OK\n");
|
|
}
|
|
|
|
if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0)
|
|
{
|
|
bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength);
|
|
bf->setFileDNAisMemoryDNA();
|
|
bf->parse(false);
|
|
m_data->m_robotMultiBodyData.push_back(bf);
|
|
|
|
for (int i=0;i<bf->m_multiBodies.size();i++)
|
|
{
|
|
int flag = bf->getFlags();
|
|
int qOffset = 7;
|
|
int uOffset=6;
|
|
|
|
if ((flag&bParse::FD_DOUBLE_PRECISION)!=0)
|
|
{
|
|
Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i];
|
|
if (mb->m_baseName)
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
|
|
}
|
|
}
|
|
|
|
for (int link=0;link<mb->m_numLinks;link++)
|
|
{
|
|
{
|
|
b3JointInfo info;
|
|
info.m_flags = 0;
|
|
info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1;
|
|
info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1;
|
|
|
|
if (mb->m_links[link].m_linkName)
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
|
|
}
|
|
info.m_linkName = mb->m_links[link].m_linkName;
|
|
}
|
|
if (mb->m_links[link].m_jointName)
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
|
|
}
|
|
info.m_jointName = mb->m_links[link].m_jointName;
|
|
info.m_jointType = mb->m_links[link].m_jointType;
|
|
}
|
|
if ((mb->m_links[link].m_jointType == eRevoluteType)||
|
|
(mb->m_links[link].m_jointType == ePrismaticType))
|
|
{
|
|
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
|
|
}
|
|
m_data->m_jointInfo.push_back(info);
|
|
}
|
|
qOffset+= mb->m_links[link].m_posVarCount;
|
|
uOffset+= mb->m_links[link].m_dofCount;
|
|
|
|
}
|
|
|
|
} else
|
|
{
|
|
Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i];
|
|
if (mb->m_baseName)
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
|
|
}
|
|
}
|
|
for (int link=0;link<mb->m_numLinks;link++)
|
|
{
|
|
{
|
|
b3JointInfo info;
|
|
info.m_flags = 0;
|
|
info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1;
|
|
info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1;
|
|
|
|
if (mb->m_links[link].m_linkName)
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
|
|
}
|
|
info.m_linkName = mb->m_links[link].m_linkName;
|
|
}
|
|
if (mb->m_links[link].m_jointName)
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
|
|
}
|
|
info.m_jointName = mb->m_links[link].m_jointName;
|
|
info.m_jointType = mb->m_links[link].m_jointType;
|
|
}
|
|
if ((mb->m_links[link].m_jointType == eRevoluteType)||
|
|
(mb->m_links[link].m_jointType == ePrismaticType))
|
|
{
|
|
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
|
|
}
|
|
m_data->m_jointInfo.push_back(info);
|
|
}
|
|
qOffset+= mb->m_links[link].m_posVarCount;
|
|
uOffset+= mb->m_links[link].m_dofCount;
|
|
}
|
|
|
|
}
|
|
}
|
|
if (bf->ok())
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Received robot description ok!\n");
|
|
}
|
|
} else
|
|
{
|
|
b3Warning("Robot description not received");
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Server received desired state");
|
|
}
|
|
break;
|
|
}
|
|
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Server completed step simulation");
|
|
}
|
|
break;
|
|
}
|
|
case CMD_URDF_LOADING_FAILED:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Server failed loading the URDF...\n");
|
|
}
|
|
m_data->m_serverLoadUrdfOK = false;
|
|
break;
|
|
}
|
|
|
|
case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Server received bullet data stream OK\n");
|
|
}
|
|
|
|
|
|
|
|
|
|
break;
|
|
}
|
|
case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Server failed receiving bullet data stream\n");
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
|
|
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Received actual state\n");
|
|
}
|
|
SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0];
|
|
|
|
int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
|
|
int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU;
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("size Q = %d, size U = %d\n", numQ,numU);
|
|
}
|
|
char msg[1024];
|
|
|
|
{
|
|
sprintf(msg,"Q=[");
|
|
|
|
for (int i=0;i<numQ;i++)
|
|
{
|
|
if (i<numQ-1)
|
|
{
|
|
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
|
|
} else
|
|
{
|
|
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
|
|
}
|
|
}
|
|
sprintf(msg,"%s]",msg);
|
|
}
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf(msg);
|
|
}
|
|
|
|
{
|
|
sprintf(msg,"U=[");
|
|
|
|
for (int i=0;i<numU;i++)
|
|
{
|
|
if (i<numU-1)
|
|
{
|
|
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
|
|
} else
|
|
{
|
|
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
|
|
}
|
|
}
|
|
sprintf(msg,"%s]",msg);
|
|
|
|
}
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf(msg);
|
|
}
|
|
|
|
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("\n");
|
|
}
|
|
break;
|
|
}
|
|
case CMD_DEBUG_LINES_COMPLETED:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Success receiving %d debug lines",serverCmd.m_sendDebugLinesArgs.m_numDebugLines);
|
|
}
|
|
|
|
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));
|
|
|
|
m_data->m_debugLinesFrom.resize(numLines);
|
|
m_data->m_debugLinesTo.resize(numLines);
|
|
m_data->m_debugLinesColor.resize(numLines);
|
|
|
|
for (int i=0;i<numLines;i++)
|
|
{
|
|
btVector3 from(linesFrom[i*4],linesFrom[i*4+1],linesFrom[i*4+2]);
|
|
btVector3 to(linesTo[i*4],linesTo[i*4+1],linesTo[i*4+2]);
|
|
btVector3 color(linesColor[i*4],linesColor[i*4+1],linesColor[i*4+2]);
|
|
|
|
m_data->m_debugLinesFrom[i] = from;
|
|
m_data->m_debugLinesTo[i] = to;
|
|
m_data->m_debugLinesColor[i] = color;
|
|
}
|
|
break;
|
|
}
|
|
case CMD_DEBUG_LINES_OVERFLOW_FAILED:
|
|
{
|
|
b3Warning("Error receiving debug lines");
|
|
m_data->m_debugLinesFrom.resize(0);
|
|
m_data->m_debugLinesTo.resize(0);
|
|
m_data->m_debugLinesColor.resize(0);
|
|
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
b3Error("Unknown server status\n");
|
|
btAssert(0);
|
|
}
|
|
};
|
|
|
|
|
|
m_data->m_testBlock1->m_numProcessedServerCommands++;
|
|
//we don't have more than 1 command outstanding (in total, either server or client)
|
|
btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == m_data->m_testBlock1->m_numServerCommands);
|
|
|
|
if (m_data->m_testBlock1->m_numServerCommands == m_data->m_testBlock1->m_numProcessedServerCommands)
|
|
{
|
|
m_data->m_waitingForServer = false;
|
|
} else
|
|
{
|
|
m_data->m_waitingForServer = true;
|
|
}
|
|
} else
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands,
|
|
m_data->m_testBlock1->m_numProcessedServerCommands);
|
|
}
|
|
}
|
|
return hasStatus;
|
|
}
|
|
|
|
bool PhysicsClientSharedMemory::canSubmitCommand() const
|
|
{
|
|
return (m_data->m_isConnected && !m_data->m_waitingForServer);
|
|
}
|
|
|
|
bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command)
|
|
{
|
|
///at the moment we allow a maximum of 1 outstanding command, so we check for this
|
|
//once the server processed the command and returns a status, we clear the flag "m_data->m_waitingForServer" and allow submitting the next command
|
|
btAssert(!m_data->m_waitingForServer);
|
|
|
|
if (!m_data->m_waitingForServer)
|
|
{
|
|
//a reset simulation command needs special attention, cleanup state
|
|
if (command.m_type==CMD_RESET_SIMULATION)
|
|
{
|
|
for (int i=0;i<m_data->m_robotMultiBodyData.size();i++)
|
|
{
|
|
delete m_data->m_robotMultiBodyData[i];
|
|
}
|
|
m_data->m_robotMultiBodyData.clear();
|
|
|
|
m_data->m_jointInfo.clear();
|
|
}
|
|
|
|
m_data->m_testBlock1->m_clientCommands[0] = command;
|
|
m_data->m_testBlock1->m_numClientCommands++;
|
|
m_data->m_waitingForServer = true;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len)
|
|
{
|
|
btAssert(len<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
|
|
if (len>=SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
|
|
{
|
|
b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n",len,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
|
|
} else
|
|
{
|
|
for (int i=0;i<len;i++)
|
|
{
|
|
m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
const btVector3* PhysicsClientSharedMemory::getDebugLinesFrom() const
|
|
{
|
|
if (m_data->m_debugLinesFrom.size())
|
|
{
|
|
return &m_data->m_debugLinesFrom[0];
|
|
}
|
|
return 0;
|
|
}
|
|
const btVector3* PhysicsClientSharedMemory::getDebugLinesTo() const
|
|
{
|
|
if (m_data->m_debugLinesTo.size())
|
|
{
|
|
return &m_data->m_debugLinesTo[0];
|
|
}
|
|
return 0;
|
|
}
|
|
const btVector3* PhysicsClientSharedMemory::getDebugLinesColor() const
|
|
{
|
|
if (m_data->m_debugLinesColor.size())
|
|
{
|
|
return &m_data->m_debugLinesColor[0];
|
|
}
|
|
return 0;
|
|
}
|
|
int PhysicsClientSharedMemory::getNumDebugLines() const
|
|
{
|
|
return m_data->m_debugLinesFrom.size();
|
|
}
|