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

Merge pull request #479 from erwincoumans/master

Browse Source 
improvements to the shared memory physics API:
This commit is contained in:
erwincoumans
2015-09-24 23:05:04 -07:00
parent e5e71bebc9 5e4badc099
commit 99c41f3c4d
14 changed files with 759 additions and 671 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -16,6 +16,7 @@ SET(App_ExampleBrowser_SRCS
ExampleEntries.cpp
ExampleEntries.h
../SharedMemory/PhysicsServer.cpp
../SharedMemory/PhysicsClientSharedMemory.cpp
../SharedMemory/PhysicsClient.cpp
../SharedMemory/PhysicsClientC_API.cpp
../SharedMemory/PhysicsServerExample.cpp

1
examples/ExampleBrowser/premake4.lua
View File

@@ -57,6 +57,7 @@
"../SharedMemory/PhysicsServerExample.cpp",
"../SharedMemory/PhysicsClientExample.cpp",
"../SharedMemory/PhysicsServer.cpp",
"../SharedMemory/PhysicsClientSharedMemory.cpp",
"../SharedMemory/PhysicsClient.cpp",
"../SharedMemory/PosixSharedMemory.cpp",
"../SharedMemory/Win32SharedMemory.cpp",

609
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -1,610 +1,3 @@
#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 TmpFloat3
{
float m_x;
float m_y;
float m_z;
};
TmpFloat3 CreateTmpFloat3(float x,float y, float z)
{
TmpFloat3 tmp; tmp.m_x = x;tmp.m_y = y;tmp.m_z = z; return tmp;
}
struct PhysicsClientSharedMemoryInternalData
{
SharedMemoryInterface* m_sharedMemory;
SharedMemoryBlock* m_testBlock1;
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
SharedMemoryStatus m_lastServerStatus;
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;
}
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
{
SharedMemoryStatus* stat = 0;
if (!m_data->m_testBlock1)
{
return 0;
}
if (!m_data->m_waitingForServer)
{
return 0;
}
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];
m_data->m_lastServerStatus = 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_RESET_SIMULATION_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
}
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();
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;
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(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;i<numLines;i++)
{
TmpFloat3 from = CreateTmpFloat3(linesFrom[i*3],linesFrom[i*3+1],linesFrom[i*3+2]);
TmpFloat3 to = CreateTmpFloat3(linesTo[i*3],linesTo[i*3+1],linesTo[i*3+2]);
TmpFloat3 color = CreateTmpFloat3(linesColor[i*3],linesColor[i*3+1],linesColor[i*3+2]);
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;
}
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;
}
if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) && (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines>0))
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//continue requesting debug lines for drawing
command.m_type =CMD_REQUEST_DEBUG_LINES;
command.m_requestDebugLinesArguments.m_startingLineIndex = serverCmd.m_sendDebugLinesArgs.m_numDebugLines+serverCmd.m_sendDebugLinesArgs.m_startingLineIndex;
submitClientCommand(command);
return 0;
}
return &m_data->m_lastServerStatus;
} 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 0;
}
bool PhysicsClientSharedMemory::canSubmitCommand() const
{
return (m_data->m_isConnected && !m_data->m_waitingForServer);
}
struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand()
{
return &m_data->m_testBlock1->m_clientCommands[0];
}
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)
{
if (&m_data->m_testBlock1->m_clientCommands[0] != &command)
{
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 float* PhysicsClientSharedMemory::getDebugLinesFrom() const
{
if (m_data->m_debugLinesFrom.size())
{
return &m_data->m_debugLinesFrom[0].m_x;
}
return 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesTo() const
{
if (m_data->m_debugLinesTo.size())
{
return &m_data->m_debugLinesTo[0].m_x;
}
return 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesColor() const
{
if (m_data->m_debugLinesColor.size())
{
return &m_data->m_debugLinesColor[0].m_x;
}
return 0;
}
int PhysicsClientSharedMemory::getNumDebugLines() const
{
return m_data->m_debugLinesFrom.size();
}
PhysicsClient::~PhysicsClient() {}

55
examples/SharedMemory/PhysicsClient.h
View File

@@ -4,46 +4,39 @@
//#include "SharedMemoryCommands.h"
#include "LinearMath/btVector3.h"
class PhysicsClientSharedMemory
{
struct PhysicsClientSharedMemoryInternalData* m_data;
protected:
class PhysicsClient {
public:
virtual ~PhysicsClient();
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
// return true if connection succesfull, can also check 'isConnected'
virtual bool connect() = 0;
//return true if connection succesfull, can also check 'isConnected'
virtual bool connect();
virtual void disconnectSharedMemory ();
virtual void disconnectSharedMemory() = 0;
virtual bool isConnected() const;
virtual bool isConnected() const = 0;
// return true if there is a status, and fill in 'serverStatus'
virtual const struct SharedMemoryStatus* processServerStatus();
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus() = 0;
virtual int getNumJoints() const;
virtual void getJointInfo(int index, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand() = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len);
virtual bool canSubmitCommand() const = 0;
virtual int getNumDebugLines() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual int getNumJoints() const = 0;
virtual void getJointInfo(int index, struct b3JointInfo& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;
virtual int getNumDebugLines() const = 0;
virtual const float* getDebugLinesFrom() const = 0;
virtual const float* getDebugLinesTo() const = 0;
virtual const float* getDebugLinesColor() const = 0;
};
#endif //BT_PHYSICS_CLIENT_API_H
#endif // BT_PHYSICS_CLIENT_API_H

88
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1,5 +1,5 @@
#include "PhysicsClientC_API.h"
#include "PhysicsClient.h"
#include "PhysicsClientSharedMemory.h"
#include "Bullet3Common/b3Scalar.h"
#include <string.h>
#include "SharedMemoryCommands.h"
@@ -8,7 +8,7 @@
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
@@ -70,7 +70,7 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -101,7 +101,7 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -113,7 +113,7 @@ b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle ph
b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -128,7 +128,7 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand
b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -139,11 +139,11 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
return (b3SharedMemoryCommandHandle) command;
}
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[dofIndex] = value;
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
return 0;
}
@@ -191,7 +191,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -200,9 +200,22 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
return (b3SharedMemoryCommandHandle) command;
}
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3JointInfo info;
b3GetJointInfo(physClient, jointIndex, &info);
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
}
b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -257,7 +270,7 @@ int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle,
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -310,13 +323,13 @@ b3PhysicsClientHandle b3ConnectSharedMemory(int key)
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
delete cl;
}
b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
const SharedMemoryStatus* stat = cl->processServerStatus();
return (b3SharedMemoryStatusHandle) stat;
@@ -339,14 +352,14 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
int b3CanSubmitCommand(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
return (int)cl->canSubmitCommand();
}
int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
return (int)cl->submitClientCommand(*command);
}
@@ -368,22 +381,58 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
int b3GetNumJoints(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumJoints();
}
void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
cl->getJointInfo(linkIndex,*info);
}
int b3PickBody(struct SharedMemoryCommand *command,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)
{
b3Assert(command);
b3Assert(command->m_type == CMD_PICK_BODY);
command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
return 0;
}
int b3MovePickedBody(struct SharedMemoryCommand *command,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)
{
b3Assert(command);
b3Assert(command->m_type == CMD_MOVE_PICKED_BODY);
command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
return 0;
}
int b3RemovePickingConstraint(b3SharedMemoryCommandHandle commandHandle)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REMOVE_PICKING_CONSTRAINT_BODY);
return 0;
}
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -396,7 +445,7 @@ b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle
}
void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(lines);
if (lines)
@@ -409,4 +458,3 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
}
}

14
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -63,11 +63,11 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
//Only use when controlMode is CONTROL_MODE_VELOCITY
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE,
int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
@@ -85,11 +85,19 @@ int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle,
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int dofIndex, int enable);
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient);
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
int b3PickBody(struct SharedMemoryCommand *command,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
int b3MovePickedBody(struct SharedMemoryCommand *command,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
int b3RemovePickingConstraint(struct SharedMemoryCommand *command);
#ifdef __cplusplus

8
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -5,7 +5,7 @@
#include "SharedMemoryCommon.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsClientC_API.h"
#include "PhysicsClient.h"
//#include "SharedMemoryCommands.h"
@@ -154,8 +154,6 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
cl->enqueueCommand(buttonId);
}
}
void PhysicsClientExample::enqueueCommand(int commandId)
@@ -177,9 +175,9 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
double startPosX = 0;
double startPosY = 0;
double startPosZ = 0;
int ret = b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
ret = b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}

527
examples/SharedMemory/PhysicsClientSharedMemory.cpp Normal file
View File

@@ -0,0 +1,527 @@
#include "PhysicsClientSharedMemory.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 TmpFloat3 {
float m_x;
float m_y;
float m_z;
};
TmpFloat3 CreateTmpFloat3(float x, float y, float z) {
TmpFloat3 tmp;
tmp.m_x = x;
tmp.m_y = y;
tmp.m_z = z;
return tmp;
}
struct PhysicsClientSharedMemoryInternalData {
SharedMemoryInterface* m_sharedMemory;
SharedMemoryBlock* m_testBlock1;
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
SharedMemoryStatus m_lastServerStatus;
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;
}
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
SharedMemoryStatus* stat = 0;
if (!m_data->m_testBlock1) {
return 0;
}
if (!m_data->m_waitingForServer) {
return 0;
}
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];
m_data->m_lastServerStatus = 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_jointIndex = link;
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_jointIndex = link;
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_RESET_SIMULATION_COMPLETED: {
if (m_data->m_verboseOutput) {
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
}
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();
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;
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(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; i < numLines; i++) {
TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1],
linesFrom[i * 3 + 2]);
TmpFloat3 to =
CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]);
TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1],
linesColor[i * 3 + 2]);
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;
}
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;
}
if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) &&
(serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) {
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
// continue requesting debug lines for drawing
command.m_type = CMD_REQUEST_DEBUG_LINES;
command.m_requestDebugLinesArguments.m_startingLineIndex =
serverCmd.m_sendDebugLinesArgs.m_numDebugLines +
serverCmd.m_sendDebugLinesArgs.m_startingLineIndex;
submitClientCommand(command);
return 0;
}
return &m_data->m_lastServerStatus;
} 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 0;
}
bool PhysicsClientSharedMemory::canSubmitCommand() const {
return (m_data->m_isConnected && !m_data->m_waitingForServer);
}
struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() {
return &m_data->m_testBlock1->m_clientCommands[0];
}
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) {
if (&m_data->m_testBlock1->m_clientCommands[0] != &command) {
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 float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
if (m_data->m_debugLinesFrom.size()) {
return &m_data->m_debugLinesFrom[0].m_x;
}
return 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesTo() const {
if (m_data->m_debugLinesTo.size()) {
return &m_data->m_debugLinesTo[0].m_x;
}
return 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesColor() const {
if (m_data->m_debugLinesColor.size()) {
return &m_data->m_debugLinesColor[0].m_x;
}
return 0;
}
int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); }

48
examples/SharedMemory/PhysicsClientSharedMemory.h Normal file
View File

@@ -0,0 +1,48 @@
#ifndef BT_PHYSICS_CLIENT_SHARED_MEMORY_API_H
#define BT_PHYSICS_CLIENT_SHARED_MEMORY_API_H
#include "PhysicsClient.h"
//#include "SharedMemoryCommands.h"
#include "LinearMath/btVector3.h"
class PhysicsClientSharedMemory : public PhysicsClient {
struct PhysicsClientSharedMemoryInternalData* m_data;
protected:
public:
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
// return true if connection succesfull, can also check 'isConnected'
virtual bool connect();
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus();
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumJoints() const;
virtual void getJointInfo(int index, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);
virtual void uploadBulletFileToSharedMemory(const char* data, int len);
virtual int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
};
#endif // BT_PHYSICS_CLIENT_API_H

56
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -1067,8 +1067,20 @@ void PhysicsServerSharedMemory::processClientCommands()
{
b3Printf("Server Init Pose not implemented yet");
}
///@todo: implement this
m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0));
int body_unique_id = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
if (m_data->m_dynamicsWorld->getNumMultibodies()>body_unique_id)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(body_unique_id);
mb->setBasePos(btVector3(
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[2]));
mb->setWorldToBaseRot(btQuaternion(
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[3],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[4],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[5],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[6]));
}
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
@@ -1136,6 +1148,46 @@ void PhysicsServerSharedMemory::processClientCommands()
break;
}
case CMD_PICK_BODY:
{
pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0],
clientCmd.m_pickBodyArguments.m_rayFromWorld[1],
clientCmd.m_pickBodyArguments.m_rayFromWorld[2]),
btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0],
clientCmd.m_pickBodyArguments.m_rayToWorld[1],
clientCmd.m_pickBodyArguments.m_rayToWorld[2]));
SharedMemoryStatus &serverCmd =
m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,
clientCmd.m_sequenceNumber, timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_MOVE_PICKED_BODY:
{
movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0],
clientCmd.m_pickBodyArguments.m_rayFromWorld[1],
clientCmd.m_pickBodyArguments.m_rayFromWorld[2]),
btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0],
clientCmd.m_pickBodyArguments.m_rayToWorld[1],
clientCmd.m_pickBodyArguments.m_rayToWorld[2]));
SharedMemoryStatus &serverCmd =
m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,
clientCmd.m_sequenceNumber, timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_REMOVE_PICKING_CONSTRAINT_BODY:
{
removePickingConstraint();
SharedMemoryStatus &serverCmd =
m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,
clientCmd.m_sequenceNumber, timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
default:
{
b3Error("Unknown command encountered");

8
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -25,7 +25,6 @@
#endif
#define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 256
#define MAX_NUM_SENSORS 256
@@ -92,6 +91,12 @@ struct SendDebugLinesArgs
int m_numRemainingDebugLines;
};
struct PickBodyArgs
{
double m_rayFromWorld[3];
double m_rayToWorld[3];
};
///Controlling a robot involves sending the desired state to its joint motor controllers.
///The control mode determines the state variables used for motor control.
@@ -225,6 +230,7 @@ struct SharedMemoryCommand
struct CreateSensorArgs m_createSensorArguments;
struct CreateBoxShapeArgs m_createBoxShapeArguments;
struct RequestDebugLinesArgs m_requestDebugLinesArguments;
struct PickBodyArgs m_pickBodyArguments;
};
};

12
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -20,6 +20,9 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_DEBUG_LINES,
CMD_STEP_FORWARD_SIMULATION,
CMD_RESET_SIMULATION,
CMD_PICK_BODY,
CMD_MOVE_PICKED_BODY,
CMD_REMOVE_PICKING_CONSTRAINT_BODY,
CMD_MAX_CLIENT_COMMANDS
};
@@ -61,10 +64,17 @@ struct b3JointInfo
int m_jointType;
int m_qIndex;
int m_uIndex;
///
int m_jointIndex;
int m_flags;
};
struct b3JointSensorState
{
double m_jointPosition;
double m_jointVelocity;
double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
};
struct b3DebugLines
{
int m_numDebugLines;

1
examples/SharedMemory/premake4.lua
View File

@@ -17,6 +17,7 @@ language "C++"
files {
"PhysicsClient.cpp",
"PhysicsClientSharedMemory.cpp",
"PhysicsClientExample.cpp",
"PhysicsServerExample.cpp",
"main.cpp",

2
test/SharedMemory/premake4.lua
View File

@@ -14,6 +14,8 @@
"test.c",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
"../../examples/SharedMemory/PhysicsClientC_API.h",
"../../examples/SharedMemory/Win32SharedMemory.cpp",