AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #2679 from erwincoumans/master

Browse Source 
Embed TCP remote graphics server in PyBullet (enable WFH), fixes for compilation on Visual C++ for Python 9.0
This commit is contained in:
erwincoumans 2020-03-22 14:04:35 -07:00 committed by GitHub
commit eab851635e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
34 changed files with 53696 additions and 52062 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Extras/BulletRobotics/premake4.lua
View File

@ -71,7 +71,9 @@ if not _OPTIONS["no-enet"] then
links {"clsocket"}
files {
"../../examples/SharedMemory/RemoteGUIHelperTCP.cpp",
"../../examples/SharedMemory/PhysicsClientTCP.cpp",
"../../examples/SharedMemory/GraphicsServerExample.cpp",
"../../examples/SharedMemory/PhysicsClientTCP.h",
"../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp",
"../../examples/SharedMemory/PhysicsClientTCP_C_API.h",

1
Extras/BulletRoboticsGUI/CMakeLists.txt
View File

@ -113,6 +113,7 @@ SET(BulletRoboticsGUI_SRCS ${BulletRoboticsGUI_INCLUDES}
../../examples/SharedMemory/GraphicsServerExample.cpp
../../examples/SharedMemory/GraphicsClientExample.cpp
../../examples/SharedMemory/RemoteGUIHelper.cpp
../../examples/SharedMemory/RemoteGUIHelperTCP.cpp
../../examples/SharedMemory/GraphicsServerExample.h
../../examples/SharedMemory/GraphicsClientExample.h
../../examples/SharedMemory/RemoteGUIHelper.h

3
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@ -30,6 +30,8 @@ void ExampleBrowserMemoryReleaseFunc(void* ptr);
#include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
#include "../SharedMemory/GraphicsServerExample.h"
#include "../SharedMemory/PhysicsClientExample.h"
@ -122,6 +124,7 @@ static ExampleEntryPhysicsServer gDefaultExamplesPhysicsServer[] =
PhysicsServerCreateFuncBullet2, PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
ExampleEntryPhysicsServer(1, "Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
PhysicsServerCreateFuncBullet2, PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
ExampleEntryPhysicsServer(1, "Graphics Server", "Create a graphics server", GraphicsServerCreateFuncBullet),
};

103488
examples/OpenGLWindow/ShapeData.h
View File

File diff suppressed because it is too large Load Diff

2
examples/RobotSimulator/CMakeLists.txt
View File

@ -18,6 +18,7 @@ RobotSimulatorMain.cpp
../SharedMemory/GraphicsServerExample.cpp
../SharedMemory/GraphicsClientExample.cpp
../SharedMemory/RemoteGUIHelper.cpp
../SharedMemory/RemoteGUIHelperTCP.cpp
../SharedMemory/GraphicsServerExample.h
../SharedMemory/GraphicsClientExample.h
../SharedMemory/RemoteGUIHelper.h
@ -32,6 +33,7 @@ RobotSimulatorMain.cpp
IF(BUILD_CLSOCKET)
ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET)
ADD_DEFINITIONS(${OSDEF})
ENDIF(BUILD_CLSOCKET)

850
examples/SharedMemory/GraphicsServerExample.cpp
View File

File diff suppressed because it is too large Load Diff

2
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@ -243,7 +243,7 @@ PhysicsClientSharedMemory::PhysicsClientSharedMemory()
PhysicsClientSharedMemory::~PhysicsClientSharedMemory()
{
if (m_data->m_isConnected)
if (m_data->m_isConnected)
{
disconnectSharedMemory();
}

3
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@ -8,9 +8,10 @@
class PhysicsClientSharedMemory : public PhysicsClient
{
struct PhysicsClientSharedMemoryInternalData* m_data;
protected:
struct PhysicsClientSharedMemoryInternalData* m_data;
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void resetData();

5
examples/SharedMemory/RemoteGUIHelper.cpp
View File

@ -218,6 +218,11 @@ RemoteGUIHelper::~RemoteGUIHelper()
delete m_data;
}
bool RemoteGUIHelper::isConnected() const
{
return m_data->isConnected();
}
void RemoteGUIHelper::setVisualizerFlag(int flag, int enable)
{
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();

2
examples/SharedMemory/RemoteGUIHelper.h
View File

@ -12,6 +12,8 @@ struct RemoteGUIHelper : public GUIHelperInterface
virtual ~RemoteGUIHelper();
bool isConnected() const;
virtual void setVisualizerFlag(int flag, int enable);
virtual void createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color);

647
examples/SharedMemory/RemoteGUIHelperTCP.cpp Normal file
View File

@ -0,0 +1,647 @@
#include "RemoteGUIHelperTCP.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "Bullet3Common/b3Logging.h"
#include "GraphicsSharedMemoryCommands.h"
#include "GraphicsSharedMemoryBlock.h"
#include "Bullet3Common/b3Scalar.h"
#include "LinearMath/btMinMax.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#include "ActiveSocket.h"
#include <string>
static unsigned int b3DeserializeInt3(const unsigned char* input)
{
unsigned int tmp = (input[3] << 24) + (input[2] << 16) + (input[1] << 8) + input[0];
return tmp;
}
static bool gVerboseNetworkMessagesClient3 = false;
const char* cmd2txt[]=
{
"GFX_CMD_INVALID",
"GFX_CMD_0",
"GFX_CMD_SET_VISUALIZER_FLAG",
"GFX_CMD_UPLOAD_DATA",
"GFX_CMD_REGISTER_TEXTURE",
"GFX_CMD_REGISTER_GRAPHICS_SHAPE",
"GFX_CMD_REGISTER_GRAPHICS_INSTANCE",
"GFX_CMD_SYNCHRONIZE_TRANSFORMS",
"GFX_CMD_REMOVE_ALL_GRAPHICS_INSTANCES",
"GFX_CMD_REMOVE_SINGLE_GRAPHICS_INSTANCE",
"GFX_CMD_CHANGE_RGBA_COLOR",
"GFX_CMD_GET_CAMERA_INFO",
//don't go beyond this command!
"GFX_CMD_MAX_CLIENT_COMMANDS",
};
struct RemoteGUIHelperTCPInternalData
{
// GUIHelperInterface* m_guiHelper;
bool m_waitingForServer;
std::string m_hostName;
int m_port;
GraphicsSharedMemoryStatus m_lastServerStatus;
CActiveSocket m_tcpSocket;
bool m_isConnected;
b3AlignedObjectArray<unsigned char> m_tempBuffer;
GraphicsSharedMemoryStatus m_lastStatus;
GraphicsSharedMemoryCommand m_command;
double m_timeOutInSeconds;
b3AlignedObjectArray<char> m_stream;
RemoteGUIHelperTCPInternalData(const char* hostName, int port)
: m_waitingForServer(false),
m_hostName(hostName),
m_port(port),
m_timeOutInSeconds(60.)
{
m_isConnected = false;
connect();
}
virtual ~RemoteGUIHelperTCPInternalData()
{
disconnect();
}
virtual bool isConnected()
{
return m_isConnected;
}
bool canSubmitCommand() const
{
if (m_isConnected && !m_waitingForServer)
{
return true;
}
return false;
}
struct GraphicsSharedMemoryCommand* getAvailableSharedMemoryCommand()
{
static int sequence = 0;
m_command.m_sequenceNumber = sequence++;
return &m_command;
}
bool submitClientCommand(const GraphicsSharedMemoryCommand& command)
{
if (gVerboseNetworkMessagesClient3)
printf("submitClientCommand: %d %s\n", command.m_type, cmd2txt[command.m_type]);
/// 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_waitingForServer);
if (!m_waitingForServer)
{
int sz = 0;
unsigned char* data = 0;
m_tempBuffer.clear();
sz = sizeof(GraphicsSharedMemoryCommand);
data = (unsigned char*)&command;
//printf("submit command of type %d\n", command.m_type);
m_tcpSocket.Send((const uint8*)data, sz);
m_waitingForServer = true;
return true;
}
return false;
}
const GraphicsSharedMemoryStatus* processServerStatus()
{
bool hasStatus = false;
//int serviceResult = enet_host_service(m_client, &m_event, 0);
int maxLen = 4 + sizeof(GraphicsSharedMemoryStatus) + GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE;
int rBytes = m_tcpSocket.Receive(maxLen);
if (rBytes <= 0)
return 0;
//append to tmp buffer
//recBytes
unsigned char* d2 = (unsigned char*)m_tcpSocket.GetData();
int curSize = m_tempBuffer.size();
m_tempBuffer.resize(curSize + rBytes);
for (int i = 0; i < rBytes; i++)
{
m_tempBuffer[curSize + i] = d2[i];
}
int packetSizeInBytes = -1;
if (m_tempBuffer.size() >= 4)
{
packetSizeInBytes = b3DeserializeInt3(&m_tempBuffer[0]);
}
if (m_tempBuffer.size() == packetSizeInBytes)
{
unsigned char* data = &m_tempBuffer[0];
if (gVerboseNetworkMessagesClient3)
{
printf("A packet of length %d bytes received\n", m_tempBuffer.size());
}
hasStatus = true;
GraphicsSharedMemoryStatus* statPtr = (GraphicsSharedMemoryStatus*)&data[4];
#if 0
if (statPtr->m_type == CMD_STEP_FORWARD_SIMULATION_COMPLETED)
{
GraphicsSharedMemoryStatus dummy;
dummy.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_lastStatus = dummy;
m_stream.resize(0);
}
else
#endif
{
m_lastStatus = *statPtr;
int streamOffsetInBytes = 4 + sizeof(GraphicsSharedMemoryStatus);
int numStreamBytes = packetSizeInBytes - streamOffsetInBytes;
m_stream.resize(numStreamBytes);
for (int i = 0; i < numStreamBytes; i++)
{
m_stream[i] = data[i + streamOffsetInBytes];
}
}
m_tempBuffer.clear();
m_waitingForServer = false;
if (gVerboseNetworkMessagesClient3)
printf("processServerStatus: %d\n", m_lastStatus.m_type);
return &m_lastStatus;
}
return 0;
}
bool connect()
{
if (m_isConnected)
return true;
m_tcpSocket.Initialize();
m_isConnected = m_tcpSocket.Open(m_hostName.c_str(), m_port);
if (m_isConnected)
{
m_tcpSocket.SetSendTimeout(m_timeOutInSeconds, 0);
m_tcpSocket.SetReceiveTimeout(m_timeOutInSeconds, 0);
}
int key = GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER;
m_tcpSocket.Send((uint8*)&key, 4);
m_tcpSocket.SetBlocking();
return m_isConnected;
}
void disconnect()
{
const char msg[16] = "disconnect";
m_tcpSocket.Send((const uint8*)msg, 10);
m_tcpSocket.Close();
m_isConnected = false;
}
};
RemoteGUIHelperTCP::RemoteGUIHelperTCP(const char* hostName, int port)
{
m_data = new RemoteGUIHelperTCPInternalData(hostName, port);
if (m_data->canSubmitCommand())
{
removeAllGraphicsInstances();
}
}
RemoteGUIHelperTCP::~RemoteGUIHelperTCP()
{
delete m_data;
}
void RemoteGUIHelperTCP::setVisualizerFlag(int flag, int enable)
{
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
cmd->m_updateFlags = 0;
cmd->m_visualizerFlagCommand.m_visualizerFlag = flag;
cmd->m_visualizerFlagCommand.m_enable = enable;
cmd->m_type = GFX_CMD_SET_VISUALIZER_FLAG;
m_data->submitClientCommand(*cmd);
}
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
}
void RemoteGUIHelperTCP::createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color)
{
printf("todo: createRigidBodyGraphicsObject\n");
}
bool RemoteGUIHelperTCP::getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float camTarget[3]) const
{
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
cmd->m_updateFlags = 0;
cmd->m_type = GFX_CMD_GET_CAMERA_INFO;
m_data->submitClientCommand(*cmd);
}
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
if (status->m_type == GFX_CMD_GET_CAMERA_INFO_COMPLETED)
{
*width = status->m_getCameraInfoStatus.width;
*height = status->m_getCameraInfoStatus.height;
for (int i = 0; i < 16; i++)
{
viewMatrix[i] = status->m_getCameraInfoStatus.viewMatrix[i];
projectionMatrix[i] = status->m_getCameraInfoStatus.projectionMatrix[i];
}
for (int i = 0; i < 3; i++)
{
camUp[i] = status->m_getCameraInfoStatus.camUp[i];
camForward[i] = status->m_getCameraInfoStatus.camForward[i];
hor[i] = status->m_getCameraInfoStatus.hor[i];
vert[i] = status->m_getCameraInfoStatus.vert[i];
camTarget[i] = status->m_getCameraInfoStatus.camTarget[i];
}
*yaw = status->m_getCameraInfoStatus.yaw;
*pitch = status->m_getCameraInfoStatus.pitch;
*camDist = status->m_getCameraInfoStatus.camDist;
return true;
}
return false;
}
void RemoteGUIHelperTCP::createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color)
{
if (body->getUserIndex() < 0)
{
btCollisionShape* shape = body->getCollisionShape();
btTransform startTransform = body->getWorldTransform();
int graphicsShapeId = shape->getUserIndex();
if (graphicsShapeId >= 0)
{
// btAssert(graphicsShapeId >= 0);
//the graphics shape is already scaled
float localScaling[4] = {1.f, 1.f, 1.f, 1.f};
float colorRGBA[4] = {(float)color[0], (float)color[1], (float)color[2], (float)color[3]};
float pos[4] = {(float)startTransform.getOrigin()[0], (float)startTransform.getOrigin()[1], (float)startTransform.getOrigin()[2], (float)startTransform.getOrigin()[3]};
float orn[4] = {(float)startTransform.getRotation()[0], (float)startTransform.getRotation()[1], (float)startTransform.getRotation()[2], (float)startTransform.getRotation()[3]};
int graphicsInstanceId = registerGraphicsInstance(graphicsShapeId, pos, orn, colorRGBA, localScaling);
body->setUserIndex(graphicsInstanceId);
}
}
}
void RemoteGUIHelperTCP::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
{
printf("todo; createCollisionShapeGraphicsObject\n");
}
void RemoteGUIHelperTCP::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
{
}
void RemoteGUIHelperTCP::syncPhysicsToGraphics2(const btDiscreteDynamicsWorld* rbWorld)
{
b3AlignedObjectArray<GUISyncPosition> updatedPositions;
int numCollisionObjects = rbWorld->getNumCollisionObjects();
{
B3_PROFILE("write all InstanceTransformToCPU2");
for (int i = 0; i < numCollisionObjects; i++)
{
//B3_PROFILE("writeSingleInstanceTransformToCPU");
btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
btCollisionShape* collisionShape = colObj->getCollisionShape();
btVector3 pos = colObj->getWorldTransform().getOrigin();
btQuaternion orn = colObj->getWorldTransform().getRotation();
int index = colObj->getUserIndex();
if (index >= 0)
{
GUISyncPosition p;
p.m_graphicsInstanceId = index;
for (int q = 0; q < 4; q++)
{
p.m_pos[q] = pos[q];
p.m_orn[q] = orn[q];
}
updatedPositions.push_back(p);
}
}
}
if (updatedPositions.size())
{
syncPhysicsToGraphics2(&updatedPositions[0], updatedPositions.size());
}
}
void RemoteGUIHelperTCP::syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions)
{
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
uploadData((unsigned char*)positions, numPositions * sizeof(GUISyncPosition), 0);
cmd->m_updateFlags = 0;
cmd->m_syncTransformsCommand.m_numPositions = numPositions;
cmd->m_type = GFX_CMD_SYNCHRONIZE_TRANSFORMS;
m_data->submitClientCommand(*cmd);
}
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
}
void RemoteGUIHelperTCP::render(const btDiscreteDynamicsWorld* rbWorld)
{
}
void RemoteGUIHelperTCP::createPhysicsDebugDrawer(btDiscreteDynamicsWorld* rbWorld)
{
}
int RemoteGUIHelperTCP::uploadData(const unsigned char* data, int sizeInBytes, int slot)
{
int chunkSize = 1024;// GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE;
int remainingBytes = sizeInBytes;
int offset = 0;
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
cmd->m_updateFlags = 0;
cmd->m_type = GFX_CMD_UPLOAD_DATA;
cmd->m_uploadDataCommand.m_numBytes = sizeInBytes;
cmd->m_uploadDataCommand.m_dataOffset = offset;
cmd->m_uploadDataCommand.m_dataSlot = slot;
m_data->submitClientCommand(*cmd);
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
while (remainingBytes > 0)
{
int curBytes = btMin(remainingBytes, chunkSize);
m_data->m_tcpSocket.Send((const uint8*)data+offset, curBytes);
if (gVerboseNetworkMessagesClient3)
printf("sending %d bytes\n", curBytes);
remainingBytes -= curBytes;
offset += curBytes;
}
if (gVerboseNetworkMessagesClient3)
printf("send all bytes!\n");
status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
return 0;
}
int RemoteGUIHelperTCP::registerTexture(const unsigned char* texels, int width, int height)
{
int textureId = -1;
//first upload all data
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
int sizeInBytes = width * height * 3; //rgb
uploadData(texels, sizeInBytes, 0);
cmd->m_updateFlags = 0;
cmd->m_type = GFX_CMD_REGISTER_TEXTURE;
cmd->m_registerTextureCommand.m_width = width;
cmd->m_registerTextureCommand.m_height = height;
m_data->submitClientCommand(*cmd);
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
if (status->m_type == GFX_CMD_REGISTER_TEXTURE_COMPLETED)
{
textureId = status->m_registerTextureStatus.m_textureId;
}
}
return textureId;
}
int RemoteGUIHelperTCP::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId)
{
int shapeId = -1;
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
uploadData((unsigned char*)vertices, numvertices * 9 * sizeof(float), 0);
uploadData((unsigned char*)indices, numIndices * sizeof(int), 1);
cmd->m_type = GFX_CMD_REGISTER_GRAPHICS_SHAPE;
cmd->m_updateFlags = 0;
cmd->m_registerGraphicsShapeCommand.m_numVertices = numvertices;
cmd->m_registerGraphicsShapeCommand.m_numIndices = numIndices;
cmd->m_registerGraphicsShapeCommand.m_primitiveType = primitiveType;
cmd->m_registerGraphicsShapeCommand.m_textureId = textureId;
m_data->submitClientCommand(*cmd);
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
if (status->m_type == GFX_CMD_REGISTER_GRAPHICS_SHAPE_COMPLETED)
{
shapeId = status->m_registerGraphicsShapeStatus.m_shapeId;
}
}
return shapeId;
}
int RemoteGUIHelperTCP::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
{
int graphicsInstanceId = -1;
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
cmd->m_type = GFX_CMD_REGISTER_GRAPHICS_INSTANCE;
cmd->m_updateFlags = 0;
cmd->m_registerGraphicsInstanceCommand.m_shapeIndex = shapeIndex;
for (int i = 0; i < 4; i++)
{
cmd->m_registerGraphicsInstanceCommand.m_position[i] = position[i];
cmd->m_registerGraphicsInstanceCommand.m_quaternion[i] = quaternion[i];
cmd->m_registerGraphicsInstanceCommand.m_color[i] = color[i];
cmd->m_registerGraphicsInstanceCommand.m_scaling[i] = scaling[i];
}
m_data->submitClientCommand(*cmd);
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
if (status->m_type == GFX_CMD_REGISTER_GRAPHICS_INSTANCE_COMPLETED)
{
graphicsInstanceId = status->m_registerGraphicsInstanceStatus.m_graphicsInstanceId;
}
}
return graphicsInstanceId;
}
void RemoteGUIHelperTCP::removeAllGraphicsInstances()
{
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
cmd->m_updateFlags = 0;
cmd->m_type = GFX_CMD_REMOVE_ALL_GRAPHICS_INSTANCES;
m_data->submitClientCommand(*cmd);
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
}
}
void RemoteGUIHelperTCP::removeGraphicsInstance(int graphicsUid)
{
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
cmd->m_updateFlags = 0;
cmd->m_type = GFX_CMD_REMOVE_SINGLE_GRAPHICS_INSTANCE;
cmd->m_removeGraphicsInstanceCommand.m_graphicsUid = graphicsUid;
m_data->submitClientCommand(*cmd);
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
}
}
void RemoteGUIHelperTCP::changeRGBAColor(int instanceUid, const double rgbaColor[4])
{
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
cmd->m_updateFlags = 0;
cmd->m_type = GFX_CMD_CHANGE_RGBA_COLOR;
cmd->m_changeRGBAColorCommand.m_graphicsUid = instanceUid;
for (int i = 0; i < 4; i++)
{
cmd->m_changeRGBAColorCommand.m_rgbaColor[i] = rgbaColor[i];
}
m_data->submitClientCommand(*cmd);
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
}
}
Common2dCanvasInterface* RemoteGUIHelperTCP::get2dCanvasInterface()
{
return 0;
}
CommonParameterInterface* RemoteGUIHelperTCP::getParameterInterface()
{
return 0;
}
CommonRenderInterface* RemoteGUIHelperTCP::getRenderInterface()
{
return 0;
}
CommonGraphicsApp* RemoteGUIHelperTCP::getAppInterface()
{
return 0;
}
void RemoteGUIHelperTCP::setUpAxis(int axis)
{
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
cmd->m_updateFlags = 0;
cmd->m_upAxisYCommand.m_enableUpAxisY = axis == 1;
cmd->m_type = GFX_CMD_0;
m_data->submitClientCommand(*cmd);
const GraphicsSharedMemoryStatus* status = 0;
while ((status = m_data->processServerStatus()) == 0)
{
}
}
}
void RemoteGUIHelperTCP::resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ)
{
}
void RemoteGUIHelperTCP::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int width, int height, int* numPixelsCopied)
{
if (numPixelsCopied)
*numPixelsCopied = 0;
}
void RemoteGUIHelperTCP::setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])
{
}
void RemoteGUIHelperTCP::setProjectiveTexture(bool useProjectiveTexture)
{
}
void RemoteGUIHelperTCP::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
{
}
void RemoteGUIHelperTCP::drawText3D(const char* txt, float posX, float posZY, float posZ, float size)
{
}
void RemoteGUIHelperTCP::drawText3D(const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag)
{
}
int RemoteGUIHelperTCP::addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid)
{
return -1;
}
void RemoteGUIHelperTCP::removeUserDebugItem(int debugItemUniqueId)
{
}
void RemoteGUIHelperTCP::removeAllUserDebugItems()
{
}

75
examples/SharedMemory/RemoteGUIHelperTCP.h Normal file
View File

@ -0,0 +1,75 @@
#ifndef REMOTE_HELPER_TCP_H
#define REMOTE_HELPER_TCP_H
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
///a RemoteGUIHelper will connect to an existing graphics server over TCP
struct RemoteGUIHelperTCP : public GUIHelperInterface
{
struct RemoteGUIHelperTCPInternalData* m_data;
RemoteGUIHelperTCP(const char* hostName, int port);
virtual ~RemoteGUIHelperTCP();
virtual void setVisualizerFlag(int flag, int enable);
virtual void createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color);
virtual void createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color);
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float camTarget[3]) const;
virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld);
virtual void syncPhysicsToGraphics2(const class btDiscreteDynamicsWorld* rbWorld);
virtual void syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions);
virtual void render(const btDiscreteDynamicsWorld* rbWorld);
virtual void createPhysicsDebugDrawer(btDiscreteDynamicsWorld* rbWorld);
virtual int registerTexture(const unsigned char* texels, int width, int height);
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId);
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
virtual void removeAllGraphicsInstances();
virtual void removeGraphicsInstance(int graphicsUid);
virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]);
virtual Common2dCanvasInterface* get2dCanvasInterface();
virtual CommonParameterInterface* getParameterInterface();
virtual CommonRenderInterface* getRenderInterface();
virtual CommonGraphicsApp* getAppInterface();
virtual void setUpAxis(int axis);
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ);
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int width, int height, int* numPixelsCopied);
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]);
virtual void setProjectiveTexture(bool useProjectiveTexture);
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld);
virtual void drawText3D(const char* txt, float posX, float posZY, float posZ, float size);
virtual void drawText3D(const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag);
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid);
virtual void removeUserDebugItem(int debugItemUniqueId);
virtual void removeAllUserDebugItems();
int uploadData(const unsigned char* data, int sizeInBytes, int slot);
};
#endif //REMOTE_HELPER_TCP_H

272
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@ -96,6 +96,8 @@ B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMain
return (b3PhysicsClientHandle)cl;
}
class InProcessPhysicsClientSharedMemory : public PhysicsClientSharedMemory
{
btInProcessExampleBrowserInternalData* m_data;
@ -127,6 +129,8 @@ public:
}
};
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, 1);
@ -148,10 +152,17 @@ class InProcessPhysicsClientExistingExampleBrowser : public PhysicsClientSharedM
SharedMemoryInterface* m_sharedMem;
b3Clock m_clock;
unsigned long long int m_prevTime;
struct GUIHelperInterface* m_guiHelper;
public:
InProcessPhysicsClientExistingExampleBrowser(struct GUIHelperInterface* guiHelper, bool useInProcessMemory, bool skipGraphicsUpdate)
InProcessPhysicsClientExistingExampleBrowser(struct GUIHelperInterface* guiHelper, bool useInProcessMemory, bool skipGraphicsUpdate, bool ownsGuiHelper)
{
m_guiHelper = 0;
if (ownsGuiHelper)
{
m_guiHelper = guiHelper;
}
m_sharedMem = 0;
CommonExampleOptions options(guiHelper);
@ -175,6 +186,7 @@ public:
//s_instancingRenderer->removeAllInstances();
delete m_physicsServerExample;
delete m_sharedMem;
delete m_guiHelper;
}
// return non-null if there is a status, nullptr otherwise
@ -253,7 +265,7 @@ B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingEx
bool useInprocessMemory = true;
bool skipGraphicsUpdate = false;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate);
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate, false);
cl->connect();
return (b3PhysicsClientHandle)cl;
@ -273,7 +285,7 @@ B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingEx
}
bool useInprocessMemory = false;
bool skipGraphicsUpdate = true;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate);
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate, false);
cl->setSharedMemoryKey(sharedMemoryKey + 1);
cl->connect();
@ -286,13 +298,15 @@ B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingEx
{
gSharedMemoryKey = sharedMemoryKey;
GUIHelperInterface* guiHelper = (GUIHelperInterface*)guiHelperPtr;
bool ownsGuiHelper = false;
if (!guiHelper)
{
guiHelper = new RemoteGUIHelper();
ownsGuiHelper = true;
}
bool useInprocessMemory = false;
bool skipGraphicsUpdate = false;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate);
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate, ownsGuiHelper);
cl->setSharedMemoryKey(sharedMemoryKey + 1);
cl->connect();
@ -301,9 +315,257 @@ B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingEx
return (b3PhysicsClientHandle)cl;
}
#ifdef BT_ENABLE_CLSOCKET
#include "RemoteGUIHelperTCP.h"
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnectTCP(const char* hostName, int port)
{
bool ownsGuiHelper = true;
GUIHelperInterface* guiHelper = new RemoteGUIHelperTCP(hostName, port);
bool useInprocessMemory = true;
bool skipGraphicsUpdate = false;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate, ownsGuiHelper);
//cl->setSharedMemoryKey(sharedMemoryKey + 1);
cl->connect();
//backward compatiblity
gSharedMemoryKey = SHARED_MEMORY_KEY;
return (b3PhysicsClientHandle)cl;
}
//backward compatiblity
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr)
{
return b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect3(guiHelperPtr, SHARED_MEMORY_KEY);
}
}
#include "SharedMemoryCommands.h"
#include "PhysicsClientSharedMemory.h"
#include "GraphicsSharedMemoryBlock.h"
#include "PosixSharedMemory.h"
#include "Win32SharedMemory.h"
class InProcessGraphicsServerSharedMemory : public PhysicsClientSharedMemory
{
btInProcessExampleBrowserInternalData* m_data2;
char** m_newargv;
SharedMemoryCommand m_command;
GraphicsSharedMemoryBlock* m_testBlock1;
SharedMemoryInterface* m_sharedMemory;
public:
InProcessGraphicsServerSharedMemory(int port)
{
int newargc = 3;
m_newargv = (char**)malloc(sizeof(void*) * newargc);
char* t0 = (char*)"--unused";
m_newargv[0] = t0;
char* t1 = (char*)"--start_demo_name=Graphics Server";
char portArg[1024];
sprintf(portArg, "--port=%d", port);
m_newargv[1] = t1;
m_newargv[2] = portArg;
bool useInProcessMemory = false;
m_data2 = btCreateInProcessExampleBrowser(newargc, m_newargv, useInProcessMemory);
SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data2);
setSharedMemoryInterface(shMem);
///////////////////
#ifdef _WIN32
m_sharedMemory = new Win32SharedMemoryServer();
#else
m_sharedMemory = new PosixSharedMemory();
#endif
/// server always has to create and initialize shared memory
bool allowCreation = false;
m_testBlock1 = (GraphicsSharedMemoryBlock*)m_sharedMemory->allocateSharedMemory(
GRAPHICS_SHARED_MEMORY_KEY, GRAPHICS_SHARED_MEMORY_SIZE, allowCreation);
}
virtual ~InProcessGraphicsServerSharedMemory()
{
m_sharedMemory->releaseSharedMemory(GRAPHICS_SHARED_MEMORY_KEY, GRAPHICS_SHARED_MEMORY_SIZE);
delete m_sharedMemory;
setSharedMemoryInterface(0);
btShutDownExampleBrowser(m_data2);
free(m_newargv);
}
virtual bool canSubmitCommand() const
{
if (m_testBlock1)
{
if (m_testBlock1->m_magicId != GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER)
{
return false;
}
}
return true;
}
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand()
{
return &m_command;
}
virtual bool submitClientCommand(const struct SharedMemoryCommand& command)
{
switch (command.m_type)
{
default:
{
}
}
return true;
}
};
class InProcessGraphicsServerSharedMemoryMainThread : public PhysicsClientSharedMemory
{
btInProcessExampleBrowserMainThreadInternalData* m_data2;
char** m_newargv;
SharedMemoryCommand m_command;
GraphicsSharedMemoryBlock* m_testBlock1;
SharedMemoryInterface* m_sharedMemory;
b3Clock m_clock;
public:
InProcessGraphicsServerSharedMemoryMainThread(int port)
{
int newargc = 3;
m_newargv = (char**)malloc(sizeof(void*) * newargc);
char* t0 = (char*)"--unused";
m_newargv[0] = t0;
char* t1 = (char*)"--start_demo_name=Graphics Server";
m_newargv[1] = t1;
char portArg[1024];
sprintf(portArg, "--port=%d", port);
m_newargv[2] = portArg;
bool useInProcessMemory = false;
m_data2 = btCreateInProcessExampleBrowserMainThread(newargc, m_newargv, useInProcessMemory);
SharedMemoryInterface* shMem = btGetSharedMemoryInterfaceMainThread(m_data2);
setSharedMemoryInterface(shMem);
///////////////////
#ifdef _WIN32
m_sharedMemory = new Win32SharedMemoryServer();
#else
m_sharedMemory = new PosixSharedMemory();
#endif
/// server always has to create and initialize shared memory
bool allowCreation = false;
m_testBlock1 = (GraphicsSharedMemoryBlock*)m_sharedMemory->allocateSharedMemory(
GRAPHICS_SHARED_MEMORY_KEY, GRAPHICS_SHARED_MEMORY_SIZE, allowCreation);
m_clock.reset();
}
virtual ~InProcessGraphicsServerSharedMemoryMainThread()
{
m_sharedMemory->releaseSharedMemory(GRAPHICS_SHARED_MEMORY_KEY, GRAPHICS_SHARED_MEMORY_SIZE);
delete m_sharedMemory;
setSharedMemoryInterface(0);
btShutDownExampleBrowserMainThread(m_data2);
free(m_newargv);
}
virtual bool canSubmitCommand() const
{
btUpdateInProcessExampleBrowserMainThread(m_data2);
if (m_testBlock1)
{
if (m_testBlock1->m_magicId != GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER)
{
return false;
}
}
return true;
}
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand()
{
return &m_command;
}
virtual bool submitClientCommand(const struct SharedMemoryCommand& command)
{
switch (command.m_type)
{
default:
{
}
}
return true;
}
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus()
{
{
if (btIsExampleBrowserMainThreadTerminated(m_data2))
{
PhysicsClientSharedMemory::disconnectSharedMemory();
}
}
{
unsigned long int ms = m_clock.getTimeMilliseconds();
if (ms > 2)
{
B3_PROFILE("m_clock.reset()");
btUpdateInProcessExampleBrowserMainThread(m_data2);
m_clock.reset();
}
}
{
b3Clock::usleep(0);
}
const SharedMemoryStatus* stat = 0;
{
stat = PhysicsClientSharedMemory::processServerStatus();
}
return stat;
}
};
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessGraphicsServerAndConnectSharedMemory(int port)
{
InProcessGraphicsServerSharedMemory* cl = new InProcessGraphicsServerSharedMemory(port);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY + 1);
cl->connect();
return (b3PhysicsClientHandle)cl;
}
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessGraphicsServerAndConnectMainThreadSharedMemory(int port)
{
InProcessGraphicsServerSharedMemoryMainThread* cl = new InProcessGraphicsServerSharedMemoryMainThread(port);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY + 1);
cl->connect();
return (b3PhysicsClientHandle)cl;
}
#endif

12
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
View File

@ -11,11 +11,15 @@ extern "C"
///think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessGraphicsServerAndConnectSharedMemory(int port);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessGraphicsServerAndConnectMainThreadSharedMemory(int port);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr);
//create a shared memory physics server, with a DummyGUIHelper (no graphics)
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr);
@ -23,7 +27,9 @@ extern "C"
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect3(void* guiHelperPtr, int sharedMemoryKey);
//create a shared memory physics server, with a RemoteGUIHelper (connect to remote graphics server) and allow to set shared memory key
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect4(void* guiHelperPtr, int sharedMemoryKey);
#ifdef BT_ENABLE_CLSOCKET
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnectTCP(const char* hostName, int port);
#endif
///ignore the following APIs, they are for internal use for example browser
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle);
void b3InProcessDebugDrawInternal(b3PhysicsClientHandle clientHandle, int debugDrawMode);

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -902,6 +902,9 @@ enum eCONNECT_METHOD
eCONNECT_GRPC = 12,
eCONNECT_PHYSX=13,
eCONNECT_SHARED_MEMORY_GUI=14,
eCONNECT_GRAPHICS_SERVER = 15,
eCONNECT_GRAPHICS_SERVER_TCP = 16,
eCONNECT_GRAPHICS_SERVER_MAIN_THREAD=17
};
enum eURDF_Flags

30
examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp
View File

@ -292,11 +292,6 @@ void cMathUtil::EulerToAxisAngle(const tVector& euler, tVector& out_axis, double
}
}
tVector cMathUtil::AxisAngleToEuler(const tVector& axis, double theta)
{
tQuaternion q = AxisAngleToQuaternion(axis, theta);
return QuaternionToEuler(q);
}
tMatrix cMathUtil::DirToRotMat(const tVector& dir, const tVector& up)
{
@ -342,29 +337,6 @@ tQuaternion cMathUtil::EulerToQuaternion(const tVector& euler)
return AxisAngleToQuaternion(axis, theta);
}
tVector cMathUtil::QuaternionToEuler(const tQuaternion& q)
{
double sinr = 2.0 * (q.w() * q.x() + q.y() * q.z());
double cosr = 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
double x = std::atan2(sinr, cosr);
double sinp = 2.0 * (q.w() * q.y() - q.z() * q.x());
double y = 0;
if (fabs(sinp) >= 1)
{
y = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
}
else
{
y = asin(sinp);
}
double siny = 2.0 * (q.w() * q.z() + q.x() * q.y());
double cosy = 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
double z = std::atan2(siny, cosy);
return tVector(x, y, z, 0);
}
tQuaternion cMathUtil::AxisAngleToQuaternion(const tVector& axis, double theta)
{
@ -746,7 +718,7 @@ void cMathUtil::ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out
int n = static_cast<int>(out_x.size());
double wc = std::tan(cutoff * M_PI / sampling_rate);
double k1 = std::sqrt(2) * wc;
double k1 = std::sqrt(2.0) * wc;
double k2 = wc * wc;
double a = k2 / (1 + k1 + k2);
double b = 2 * a;

4
examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h
View File

@ -61,7 +61,7 @@ public:
static tVector RotMatToEuler(const tMatrix& mat);
static tQuaternion RotMatToQuaternion(const tMatrix& mat);
static void EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta);
static tVector AxisAngleToEuler(const tVector& axis, double theta);
static tMatrix DirToRotMat(const tVector& dir, const tVector& up);
static void DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1,
@ -69,7 +69,7 @@ public:
static tMatrix DeltaRot(const tMatrix& R0, const tMatrix& R1);
static tQuaternion EulerToQuaternion(const tVector& euler);
static tVector QuaternionToEuler(const tQuaternion& q);
static tQuaternion AxisAngleToQuaternion(const tVector& axis, double theta);
static void QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta);
static tMatrix BuildQuaternionDiffMat(const tQuaternion& q);

6
examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp
View File

@ -19,7 +19,7 @@ void cRBDModel::Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& bo
int num_dofs = GetNumDof();
int num_joints = GetNumJoints();
const int svs = cSpAlg::gSpVecSize;
const int svs = gSpVecSize;
mPose = Eigen::VectorXd::Zero(num_dofs);
mVel = Eigen::VectorXd::Zero(num_dofs);
@ -27,7 +27,7 @@ void cRBDModel::Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& bo
tMatrix trans_mat;
InitJointSubspaceArr();
mChildParentMatArr = Eigen::MatrixXd::Zero(num_joints * trans_mat.rows(), trans_mat.cols());
mSpWorldJointTransArr = Eigen::MatrixXd::Zero(num_joints * cSpAlg::gSVTransRows, cSpAlg::gSVTransCols);
mSpWorldJointTransArr = Eigen::MatrixXd::Zero(num_joints * gSVTransRows, gSVTransCols);
mMassMat = Eigen::MatrixXd::Zero(num_dofs, num_dofs);
mBiasForce = Eigen::VectorXd::Zero(num_dofs);
mInertiaBuffer = Eigen::MatrixXd::Zero(num_joints * svs, svs);
@ -206,7 +206,7 @@ void cRBDModel::InitJointSubspaceArr()
{
int num_dofs = GetNumDof();
int num_joints = GetNumJoints();
mJointSubspaceArr = Eigen::MatrixXd(cSpAlg::gSpVecSize, num_dofs);
mJointSubspaceArr = Eigen::MatrixXd(gSpVecSize, num_dofs);
for (int j = 0; j < num_joints; ++j)
{
int offset = cKinTree::GetParamOffset(mJointMat, j);

62
examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp
View File

@ -20,9 +20,9 @@ void cRBDUtil::SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc,
cSpAlg::tSpVec acc0 = cSpAlg::BuildSV(tVector::Zero(), -gravity);
int num_joints = cKinTree::GetNumJoints(joint_mat);
Eigen::MatrixXd vels = Eigen::MatrixXd(num_joints, cSpAlg::gSpVecSize);
Eigen::MatrixXd accs = Eigen::MatrixXd(num_joints, cSpAlg::gSpVecSize);
Eigen::MatrixXd fs = Eigen::MatrixXd(num_joints, cSpAlg::gSpVecSize);
Eigen::MatrixXd vels = Eigen::MatrixXd(num_joints, gSpVecSize);
Eigen::MatrixXd accs = Eigen::MatrixXd(num_joints, gSpVecSize);
Eigen::MatrixXd fs = Eigen::MatrixXd(num_joints, gSpVecSize);
for (int j = 0; j < num_joints; ++j)
{
@ -117,7 +117,7 @@ void cRBDUtil::SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau,
void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& out_mass_mat)
{
const int svs = cSpAlg::gSpVecSize;
const int svs = gSpVecSize;
int num_joints = model.GetNumJoints();
Eigen::MatrixXd Is = Eigen::MatrixXd::Zero(num_joints * svs, svs);
BuildMassMat(model, Is, out_mass_mat);
@ -134,7 +134,7 @@ void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buf
int dim = model.GetNumDof();
int num_joints = model.GetNumJoints();
H.setZero(dim, dim);
const int svs = cSpAlg::gSpVecSize;
const int svs = gSpVecSize;
Eigen::MatrixXd child_parent_mats_F = Eigen::MatrixXd(svs * num_joints, svs);
Eigen::MatrixXd parent_child_mats_M = Eigen::MatrixXd(svs * num_joints, svs);
@ -204,7 +204,7 @@ void cRBDUtil::BuildEndEffectorJacobian(const cRBDModel& model, int joint_id, Ei
const Eigen::VectorXd& pose = model.GetPose();
int num_dofs = cKinTree::GetNumDof(joint_mat);
out_J = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, num_dofs);
out_J = Eigen::MatrixXd::Zero(gSpVecSize, num_dofs);
int curr_id = joint_id;
cSpAlg::tSpTrans curr_trans = cSpAlg::BuildTrans();
@ -214,7 +214,7 @@ void cRBDUtil::BuildEndEffectorJacobian(const cRBDModel& model, int joint_id, Ei
int size = cKinTree::GetParamSize(joint_mat, curr_id);
const Eigen::MatrixXd S = model.GetJointSubspace(curr_id);
out_J.block(0, offset, cSpAlg::gSpVecSize, size) = cSpAlg::ApplyTransM(curr_trans, S);
out_J.block(0, offset, gSpVecSize, size) = cSpAlg::ApplyTransM(curr_trans, S);
int parent_id = cKinTree::GetParent(joint_mat, curr_id);
cSpAlg::tSpTrans parent_child_trans = model.GetSpParentChildTrans(curr_id);
@ -229,7 +229,7 @@ void cRBDUtil::BuildEndEffectorJacobian(const Eigen::MatrixXd& joint_mat, const
{
// jacobian in world coordinates
int num_dofs = cKinTree::GetNumDof(joint_mat);
out_J = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, num_dofs);
out_J = Eigen::MatrixXd::Zero(gSpVecSize, num_dofs);
int curr_id = joint_id;
cSpAlg::tSpTrans curr_trans = cSpAlg::BuildTrans();
@ -240,7 +240,7 @@ void cRBDUtil::BuildEndEffectorJacobian(const Eigen::MatrixXd& joint_mat, const
Eigen::MatrixXd S = BuildJointSubspace(joint_mat, pose, curr_id);
S = cSpAlg::ApplyTransM(curr_trans, S);
out_J.block(0, offset, cSpAlg::gSpVecSize, size) = S;
out_J.block(0, offset, gSpVecSize, size) = S;
int parent_id = cKinTree::GetParent(joint_mat, curr_id);
cSpAlg::tSpTrans parent_child_trans = BuildParentChildTransform(joint_mat, pose, curr_id);
@ -262,7 +262,7 @@ Eigen::MatrixXd cRBDUtil::MultJacobianEndEff(const Eigen::MatrixXd& joint_mat, c
int size = cKinTree::GetParamSize(joint_mat, curr_id);
Eigen::VectorXd curr_q;
cKinTree::GetJointParams(joint_mat, q, curr_id, curr_q);
const Eigen::Block<const Eigen::MatrixXd> curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size);
const Eigen::Block<const Eigen::MatrixXd> curr_J = J.block(0, offset, gSpVecSize, size);
joint_vel += curr_J * curr_q;
curr_id = cKinTree::GetParent(joint_mat, curr_id);
@ -276,7 +276,7 @@ void cRBDUtil::BuildJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J)
const Eigen::VectorXd& pose = model.GetPose();
int num_dofs = model.GetNumDof();
out_J = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, num_dofs);
out_J = Eigen::MatrixXd::Zero(gSpVecSize, num_dofs);
int num_joints = cKinTree::GetNumJoints(joint_mat);
for (int j = 0; j < num_joints; ++j)
@ -287,7 +287,7 @@ void cRBDUtil::BuildJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J)
int size = cKinTree::GetParamSize(joint_mat, j);
const Eigen::MatrixXd S = model.GetJointSubspace(j);
out_J.block(0, offset, cSpAlg::gSpVecSize, size) = cSpAlg::ApplyInvTransM(world_joint_trans, S);
out_J.block(0, offset, gSpVecSize, size) = cSpAlg::ApplyInvTransM(world_joint_trans, S);
}
}
@ -300,9 +300,9 @@ Eigen::MatrixXd cRBDUtil::ExtractEndEffJacobian(const Eigen::MatrixXd& joint_mat
{
int offset = cKinTree::GetParamOffset(joint_mat, curr_id);
int size = cKinTree::GetParamSize(joint_mat, curr_id);
const Eigen::Block<const Eigen::MatrixXd> curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size);
const Eigen::Block<const Eigen::MatrixXd> curr_J = J.block(0, offset, gSpVecSize, size);
J_end_eff.block(0, offset, cSpAlg::gSpVecSize, size) = curr_J;
J_end_eff.block(0, offset, gSpVecSize, size) = curr_J;
curr_id = cKinTree::GetParent(joint_mat, curr_id);
}
return J_end_eff;
@ -327,7 +327,7 @@ void cRBDUtil::BuildCOMJacobian(const cRBDModel& model, const Eigen::MatrixXd& J
const Eigen::MatrixXd& body_defs = model.GetBodyDefs();
int num_dofs = cKinTree::GetNumDof(joint_mat);
out_J = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, num_dofs);
out_J = Eigen::MatrixXd::Zero(gSpVecSize, num_dofs);
double total_mass = cKinTree::CalcTotalMass(body_defs);
int num_joints = cKinTree::GetNumJoints(joint_mat);
@ -353,8 +353,8 @@ void cRBDUtil::BuildCOMJacobian(const cRBDModel& model, const Eigen::MatrixXd& J
int offset = cKinTree::GetParamOffset(joint_mat, curr_id);
int size = cKinTree::GetParamSize(joint_mat, curr_id);
const Eigen::MatrixXd& J_block = J.block(0, offset, cSpAlg::gSpVecSize, size);
Eigen::Block<Eigen::MatrixXd, -1, -1, false> J_com_block = out_J.block(0, offset, cSpAlg::gSpVecSize, size);
const Eigen::MatrixXd& J_block = J.block(0, offset, gSpVecSize, size);
Eigen::Block<Eigen::MatrixXd, -1, -1, false> J_com_block = out_J.block(0, offset, gSpVecSize, size);
J_com_block += mass_frac * cSpAlg::ApplyTransM(body_pos_trans, J_block);
curr_id = cKinTree::GetParent(joint_mat, curr_id);
@ -373,8 +373,8 @@ void cRBDUtil::BuildJacobianDot(const cRBDModel& model, Eigen::MatrixXd& out_J_d
int num_dofs = cKinTree::GetNumDof(joint_mat);
int num_joints = cKinTree::GetNumJoints(joint_mat);
out_J_dot = Eigen::MatrixXd(cSpAlg::gSpVecSize, num_dofs);
Eigen::MatrixXd Sqs(cSpAlg::gSpVecSize, num_joints);
out_J_dot = Eigen::MatrixXd(gSpVecSize, num_dofs);
Eigen::MatrixXd Sqs(gSpVecSize, num_joints);
for (int j = 0; j < num_joints; ++j)
{
@ -395,7 +395,7 @@ void cRBDUtil::BuildJacobianDot(const cRBDModel& model, Eigen::MatrixXd& out_J_d
int offset = cKinTree::GetParamOffset(joint_mat, j);
int size = cKinTree::GetParamSize(joint_mat, j);
out_J_dot.block(0, offset, cSpAlg::gSpVecSize, size) = cSpAlg::CrossMs(parent_Sq, S);
out_J_dot.block(0, offset, gSpVecSize, size) = cSpAlg::CrossMs(parent_Sq, S);
}
}
@ -457,7 +457,7 @@ cSpAlg::tSpVec cRBDUtil::CalcVelProdAcc(const cRBDModel& model, const Eigen::Mat
Eigen::VectorXd dq;
cKinTree::GetJointParams(joint_mat, pose, curr_id, q);
cKinTree::GetJointParams(joint_mat, vel, curr_id, dq);
const Eigen::Block<const Eigen::MatrixXd> curr_Jd = Jd.block(0, offset, cSpAlg::gSpVecSize, size);
const Eigen::Block<const Eigen::MatrixXd> curr_Jd = Jd.block(0, offset, gSpVecSize, size);
cSpAlg::tSpVec cj = BuildCj(joint_mat, q, dq, curr_id);
if (cj.squaredNorm() > 0)
@ -757,11 +757,11 @@ void cRBDUtil::CalcWorldJointTransforms(const cRBDModel& model, Eigen::MatrixXd&
const Eigen::VectorXd& pose = model.GetPose();
int num_joints = cKinTree::GetNumJoints(joint_mat);
out_trans_arr.resize(num_joints * cSpAlg::gSVTransRows, cSpAlg::gSVTransCols);
out_trans_arr.resize(num_joints * gSVTransRows, gSVTransCols);
for (int j = 0; j < num_joints; ++j)
{
int row_idx = j * cSpAlg::gSVTransRows;
int row_idx = j * gSVTransRows;
int parent_id = cKinTree::GetParent(joint_mat, j);
cSpAlg::tSpTrans parent_child_trans = model.GetSpParentChildTrans(j);
@ -773,7 +773,7 @@ void cRBDUtil::CalcWorldJointTransforms(const cRBDModel& model, Eigen::MatrixXd&
}
cSpAlg::tSpTrans world_child_trans = cSpAlg::CompTrans(parent_child_trans, world_parent_trans);
out_trans_arr.block(row_idx, 0, cSpAlg::gSVTransRows, cSpAlg::gSVTransCols) = world_child_trans;
out_trans_arr.block(row_idx, 0, gSVTransRows, gSVTransCols) = world_child_trans;
}
}
@ -841,7 +841,7 @@ Eigen::MatrixXd cRBDUtil::BuildJointSubspaceRoot(const Eigen::MatrixXd& joint_ma
int pos_dim = cKinTree::gPosDim;
int rot_dim = cKinTree::gRotDim;
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(gSpVecSize, dim);
tQuaternion quat = cKinTree::GetRootRot(joint_mat, pose);
tMatrix E = cMathUtil::RotateMat(quat);
@ -854,7 +854,7 @@ Eigen::MatrixXd cRBDUtil::BuildJointSubspaceRoot(const Eigen::MatrixXd& joint_ma
Eigen::MatrixXd cRBDUtil::BuildJointSubspaceRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j)
{
int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypeRevolute);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(gSpVecSize, dim);
S(2, 0) = 1;
return S;
}
@ -862,7 +862,7 @@ Eigen::MatrixXd cRBDUtil::BuildJointSubspaceRevolute(const Eigen::MatrixXd& join
Eigen::MatrixXd cRBDUtil::BuildJointSubspacePrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j)
{
int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypePrismatic);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(gSpVecSize, dim);
S(3, 0) = 1;
return S;
@ -871,7 +871,7 @@ Eigen::MatrixXd cRBDUtil::BuildJointSubspacePrismatic(const Eigen::MatrixXd& joi
Eigen::MatrixXd cRBDUtil::BuildJointSubspacePlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j)
{
int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypePlanar);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(gSpVecSize, dim);
S(3, 0) = 1;
S(4, 1) = 1;
S(2, 2) = 1;
@ -881,14 +881,14 @@ Eigen::MatrixXd cRBDUtil::BuildJointSubspacePlanar(const Eigen::MatrixXd& joint_
Eigen::MatrixXd cRBDUtil::BuildJointSubspaceFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j)
{
int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypeFixed);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(gSpVecSize, dim);
return S;
}
Eigen::MatrixXd cRBDUtil::BuildJointSubspaceSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j)
{
int dim = cKinTree::GetJointParamSize(cKinTree::eJointTypeSpherical);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(cSpAlg::gSpVecSize, dim);
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(gSpVecSize, dim);
S(0, 0) = 1;
S(1, 1) = 1;
S(2, 2) = 1;
@ -1016,7 +1016,7 @@ void cRBDUtil::CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_f
cSpAlg::tSpVec acc0 = cSpAlg::BuildSV(tVector::Zero(), gravity);
int num_joints = cKinTree::GetNumJoints(joint_mat);
Eigen::MatrixXd fs = Eigen::MatrixXd(num_joints, cSpAlg::gSpVecSize);
Eigen::MatrixXd fs = Eigen::MatrixXd(num_joints, gSpVecSize);
for (int j = 0; j < num_joints; ++j)
{

3
examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp
View File

@ -1,9 +1,6 @@
#include "SpAlg.h"
#include <iostream>
const int cSpAlg::gSpVecSize;
const int cSpAlg::gSVTransRows;
const int cSpAlg::gSVTransCols;
cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1)
{

10
examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h
View File

@ -2,14 +2,16 @@
#include "MathUtil.h"
#define gSpVecSize 6
#define gSVTransRows 3
#define gSVTransCols 4
// spatial algebra util
class cSpAlg
{
public:
const static int gSpVecSize = 6;
const static int gSVTransRows = 3;
const static int gSVTransCols = 4;
typedef Eigen::Matrix<double, gSVTransRows, gSVTransCols> tSpTrans;
typedef Eigen::Matrix<double, gSpVecSize, 1> tSpVec;

26
examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
View File

@ -614,7 +614,7 @@ void XMLUtil::ToStr(double v, char* buffer, int bufferSize)
TIXML_SNPRINTF(buffer, bufferSize, "%.17g", v);
}
void XMLUtil::ToStr(int64_t v, char* buffer, int bufferSize)
void XMLUtil::ToStr(xml_Int64a_t v, char* buffer, int bufferSize)
{
// horrible syntax trick to make the compiler happy about %lld
TIXML_SNPRINTF(buffer, bufferSize, "%lld", (long long)v);
@ -677,12 +677,12 @@ bool XMLUtil::ToDouble(const char* str, double* value)
return false;
}
bool XMLUtil::ToInt64(const char* str, int64_t* value)
bool XMLUtil::ToInt64(const char* str, xml_Int64a_t* value)
{
long long v = 0; // horrible syntax trick to make the compiler happy about %lld
if (TIXML_SSCANF(str, "%lld", &v) == 1)
{
*value = (int64_t)v;
*value = (xml_Int64a_t)v;
return true;
}
return false;
@ -1471,7 +1471,7 @@ XMLError XMLAttribute::QueryUnsignedValue(unsigned int* value) const
return XML_WRONG_ATTRIBUTE_TYPE;
}
XMLError XMLAttribute::QueryInt64Value(int64_t* value) const
XMLError XMLAttribute::QueryInt64Value(xml_Int64a_t* value) const
{
if (XMLUtil::ToInt64(Value(), value))
{
@ -1526,7 +1526,7 @@ void XMLAttribute::SetAttribute(unsigned v)
_value.SetStr(buf);
}
void XMLAttribute::SetAttribute(int64_t v)
void XMLAttribute::SetAttribute(xml_Int64a_t v)
{
char buf[BUF_SIZE];
XMLUtil::ToStr(v, buf, BUF_SIZE);
@ -1611,9 +1611,9 @@ unsigned XMLElement::UnsignedAttribute(const char* name, unsigned defaultValue)
return i;
}
int64_t XMLElement::Int64Attribute(const char* name, int64_t defaultValue) const
xml_Int64a_t XMLElement::Int64Attribute(const char* name, xml_Int64a_t defaultValue) const
{
int64_t i = defaultValue;
xml_Int64a_t i = defaultValue;
QueryInt64Attribute(name, &i);
return i;
}
@ -1673,7 +1673,7 @@ void XMLElement::SetText(unsigned v)
SetText(buf);
}
void XMLElement::SetText(int64_t v)
void XMLElement::SetText(xml_Int64a_t v)
{
char buf[BUF_SIZE];
XMLUtil::ToStr(v, buf, BUF_SIZE);
@ -1729,7 +1729,7 @@ XMLError XMLElement::QueryUnsignedText(unsigned* uval) const
return XML_NO_TEXT_NODE;
}
XMLError XMLElement::QueryInt64Text(int64_t* ival) const
XMLError XMLElement::QueryInt64Text(xml_Int64a_t* ival) const
{
if (FirstChild() && FirstChild()->ToText())
{
@ -1799,9 +1799,9 @@ unsigned XMLElement::UnsignedText(unsigned defaultValue) const
return i;
}
int64_t XMLElement::Int64Text(int64_t defaultValue) const
xml_Int64a_t XMLElement::Int64Text(xml_Int64a_t defaultValue) const
{
int64_t i = defaultValue;
xml_Int64a_t i = defaultValue;
QueryInt64Text(&i);
return i;
}
@ -2710,7 +2710,7 @@ void XMLPrinter::PushAttribute(const char* name, unsigned v)
PushAttribute(name, buf);
}
void XMLPrinter::PushAttribute(const char* name, int64_t v)
void XMLPrinter::PushAttribute(const char* name, xml_Int64a_t v)
{
char buf[BUF_SIZE];
XMLUtil::ToStr(v, buf, BUF_SIZE);
@ -2790,7 +2790,7 @@ void XMLPrinter::PushText(const char* text, bool cdata)
}
}
void XMLPrinter::PushText(int64_t value)
void XMLPrinter::PushText(xml_Int64a_t value)
{
char buf[BUF_SIZE];
XMLUtil::ToStr(value, buf, BUF_SIZE);

50
examples/ThirdPartyLibs/tinyxml2/tinyxml2.h
View File

@ -40,7 +40,27 @@ distribution.
#include <cstdlib>
#include <cstring>
#endif
#ifdef __GNUC__
#include <stdint.h>
typedef int32_t xml_Int32a_t;
typedef int64_t xml_Int64a_t;
typedef uint32_t xml_Uint32a_t;
typedef uint64_t xml_Uint64a_t;
#elif defined(_MSC_VER)
typedef __int32 xml_Int32a_t;
typedef __int64 xml_Int64a_t;
typedef unsigned __int32 xml_Uint32a_t;
typedef unsigned __int64 xml_Uint64a_t;
#else
typedef int xml_Int32a_t;
typedef long long int xml_Int64a_t;
typedef unsigned int xml_Uint32a_t;
typedef unsigned long long int xml_Uint64a_t;
#endif
/*
TODO: intern strings instead of allocation.
@ -678,7 +698,7 @@ public:
static void ToStr(bool v, char* buffer, int bufferSize);
static void ToStr(float v, char* buffer, int bufferSize);
static void ToStr(double v, char* buffer, int bufferSize);
static void ToStr(int64_t v, char* buffer, int bufferSize);
static void ToStr(xml_Int64a_t v, char* buffer, int bufferSize);
// converts strings to primitive types
static bool ToInt(const char* str, int* value);
@ -686,7 +706,7 @@ public:
static bool ToBool(const char* str, bool* value);
static bool ToFloat(const char* str, float* value);
static bool ToDouble(const char* str, double* value);
static bool ToInt64(const char* str, int64_t* value);
static bool ToInt64(const char* str, xml_Int64a_t* value);
// Changes what is serialized for a boolean value.
// Default to "true" and "false". Shouldn't be changed
@ -1260,9 +1280,9 @@ public:
return i;
}
int64_t Int64Value() const
xml_Int64a_t Int64Value() const
{
int64_t i = 0;
xml_Int64a_t i = 0;
QueryInt64Value(&i);
return i;
}
@ -1304,7 +1324,7 @@ public:
/// See QueryIntValue
XMLError QueryUnsignedValue(unsigned int* value) const;
/// See QueryIntValue
XMLError QueryInt64Value(int64_t* value) const;
XMLError QueryInt64Value(xml_Int64a_t* value) const;
/// See QueryIntValue
XMLError QueryBoolValue(bool* value) const;
/// See QueryIntValue
@ -1319,7 +1339,7 @@ public:
/// Set the attribute to value.
void SetAttribute(unsigned value);
/// Set the attribute to value.
void SetAttribute(int64_t value);
void SetAttribute(xml_Int64a_t value);
/// Set the attribute to value.
void SetAttribute(bool value);
/// Set the attribute to value.
@ -1414,7 +1434,7 @@ public:
/// See IntAttribute()
unsigned UnsignedAttribute(const char* name, unsigned defaultValue = 0) const;
/// See IntAttribute()
int64_t Int64Attribute(const char* name, int64_t defaultValue = 0) const;
xml_Int64a_t Int64Attribute(const char* name, xml_Int64a_t defaultValue = 0) const;
/// See IntAttribute()
bool BoolAttribute(const char* name, bool defaultValue = false) const;
/// See IntAttribute()
@ -1457,7 +1477,7 @@ public:
}
/// See QueryIntAttribute()
XMLError QueryInt64Attribute(const char* name, int64_t* value) const
XMLError QueryInt64Attribute(const char* name, xml_Int64a_t* value) const
{
const XMLAttribute* a = FindAttribute(name);
if (!a)
@ -1537,7 +1557,7 @@ public:
return QueryUnsignedAttribute(name, value);
}
int QueryAttribute(const char* name, int64_t* value) const
int QueryAttribute(const char* name, xml_Int64a_t* value) const
{
return QueryInt64Attribute(name, value);
}
@ -1577,7 +1597,7 @@ public:
}
/// Sets the named attribute to value.
void SetAttribute(const char* name, int64_t value)
void SetAttribute(const char* name, xml_Int64a_t value)
{
XMLAttribute* a = FindOrCreateAttribute(name);
a->SetAttribute(value);
@ -1685,7 +1705,7 @@ public:
/// Convenience method for setting text inside an element. See SetText() for important limitations.
void SetText(unsigned value);
/// Convenience method for setting text inside an element. See SetText() for important limitations.
void SetText(int64_t value);
void SetText(xml_Int64a_t value);
/// Convenience method for setting text inside an element. See SetText() for important limitations.
void SetText(bool value);
/// Convenience method for setting text inside an element. See SetText() for important limitations.
@ -1723,7 +1743,7 @@ public:
/// See QueryIntText()
XMLError QueryUnsignedText(unsigned* uval) const;
/// See QueryIntText()
XMLError QueryInt64Text(int64_t* uval) const;
XMLError QueryInt64Text(xml_Int64a_t* uval) const;
/// See QueryIntText()
XMLError QueryBoolText(bool* bval) const;
/// See QueryIntText()
@ -1736,7 +1756,7 @@ public:
/// See QueryIntText()
unsigned UnsignedText(unsigned defaultValue = 0) const;
/// See QueryIntText()
int64_t Int64Text(int64_t defaultValue = 0) const;
xml_Int64a_t Int64Text(xml_Int64a_t defaultValue = 0) const;
/// See QueryIntText()
bool BoolText(bool defaultValue = false) const;
/// See QueryIntText()
@ -2392,7 +2412,7 @@ public:
void PushAttribute(const char* name, const char* value);
void PushAttribute(const char* name, int value);
void PushAttribute(const char* name, unsigned value);
void PushAttribute(const char* name, int64_t value);
void PushAttribute(const char* name, xml_Int64a_t value);
void PushAttribute(const char* name, bool value);
void PushAttribute(const char* name, double value);
/// If streaming, close the Element.
@ -2405,7 +2425,7 @@ public:
/// Add a text node from an unsigned.
void PushText(unsigned value);
/// Add a text node from an unsigned.
void PushText(int64_t value);
void PushText(xml_Int64a_t value);
/// Add a text node from a bool.
void PushText(bool value);
/// Add a text node from a float.

1
examples/TwoJoint/CMakeLists.txt
View File

@ -35,6 +35,7 @@ SET(RobotSimulator_SRCS
../SharedMemory/GraphicsServerExample.cpp
../SharedMemory/GraphicsClientExample.cpp
../SharedMemory/RemoteGUIHelper.cpp
../SharedMemory/RemoteGUIHelperTCP.cpp
../SharedMemory/GraphicsServerExample.h
../SharedMemory/GraphicsClientExample.h
../SharedMemory/RemoteGUIHelper.h

15
examples/pybullet/examples/graphicsClient.py Normal file
View File

@ -0,0 +1,15 @@
import pybullet as p
import time
p.connect(p.GRAPHICS_SERVER_TCP)
import pybullet_data as pd
p.setAdditionalSearchPath(pd.getDataPath())
p.loadURDF("plane.urdf")
p.loadURDF("r2d2.urdf", [0,0,3])
p.setGravity(0,0,-10)
gravId = p.addUserDebugParameter("gravity",0,10,0)
while p.isConnected():
p.stepSimulation()

8
examples/pybullet/examples/graphicsServer.py Normal file
View File

@ -0,0 +1,8 @@
import pybullet as p
import time
p.connect(p.GRAPHICS_SERVER)
#p.connect(p.GRAPHICS_SERVER_MAIN_THREAD)
while p.isConnected():
p.stepSimulation()
time.sleep(1./240.)

1
examples/pybullet/gym/pybullet_robots/laikago/__init__.py Normal file
View File

@ -0,0 +1 @@

1
examples/pybullet/gym/pybullet_robots/xarm/__init__.py Normal file
View File

@ -0,0 +1 @@

13
examples/pybullet/gym/pybullet_utils/graphicsClient.py Normal file
View File

@ -0,0 +1,13 @@
import pybullet as p
import time
p.connect(p.GRAPHICS_SERVER_TCP)
import pybullet_data as pd
p.setAdditionalSearchPath(pd.getDataPath())
p.loadURDF("plane.urdf")
p.loadURDF("r2d2.urdf", [0,0,3])
p.setGravity(0,0,-10)
while p.isConnected():
p.stepSimulation()

7
examples/pybullet/gym/pybullet_utils/graphicsServer.py Normal file
View File

@ -0,0 +1,7 @@
import pybullet as p
import time
p.connect(p.GRAPHICS_SERVER)
print("started graphics server")
while p.isConnected():
time.sleep(1./240.)

17
examples/pybullet/premake4.lua
View File

@ -124,14 +124,15 @@ if not _OPTIONS["no-enet"] then
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp",
"../SharedMemory/GraphicsClientExample.cpp",
"../SharedMemory/GraphicsClientExample.h",
"../SharedMemory/GraphicsServerExample.cpp",
"../SharedMemory/GraphicsServerExample.h",
"../SharedMemory/GraphicsSharedMemoryBlock.h",
"../SharedMemory/GraphicsSharedMemoryCommands.h",
"../SharedMemory/GraphicsSharedMemoryPublic.h",
"../SharedMemory/RemoteGUIHelper.cpp",
"../SharedMemory/RemoteGUIHelper.h",
"../SharedMemory/GraphicsClientExample.h",
"../SharedMemory/GraphicsServerExample.cpp",
"../SharedMemory/GraphicsServerExample.h",
"../SharedMemory/GraphicsSharedMemoryBlock.h",
"../SharedMemory/GraphicsSharedMemoryCommands.h",
"../SharedMemory/GraphicsSharedMemoryPublic.h",
"../SharedMemory/RemoteGUIHelper.cpp",
"../SharedMemory/RemoteGUIHelperTCP.cpp",
"../SharedMemory/RemoteGUIHelper.h",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",

85
examples/pybullet/pybullet.c
View File

@ -438,6 +438,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
}
switch (method)
{
case eCONNECT_GUI:
{
#ifdef __APPLE__
@ -458,6 +459,20 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
sm = b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(argc, argv);
#else
sm = b3CreateInProcessPhysicsServerAndConnectSharedMemory(argc, argv);
#endif
break;
}
case eCONNECT_GRAPHICS_SERVER_MAIN_THREAD:
{
sm = b3CreateInProcessGraphicsServerAndConnectMainThreadSharedMemory(tcpPort);
break;
}
case eCONNECT_GRAPHICS_SERVER:
{
#ifdef __APPLE__
sm = b3CreateInProcessGraphicsServerAndConnectMainThreadSharedMemory(tcpPort);
#else
sm = b3CreateInProcessGraphicsServerAndConnectSharedMemory(tcpPort);
#endif
break;
}
@ -473,6 +488,17 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
break;
}
case eCONNECT_GRAPHICS_SERVER_TCP:
{
#ifdef BT_ENABLE_CLSOCKET
sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnectTCP(hostName, tcpPort);
#else
PyErr_SetString(SpamError, "TCP is not enabled in this pybullet build");
return NULL;
#endif//BT_ENABLE_CLSOCKET
break;
}
case eCONNECT_DIRECT:
{
@ -576,34 +602,37 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
sPhysicsClientsGUI[freeIndex] = method;
sNumPhysicsClients++;
command = b3InitSyncBodyInfoCommand(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SYNC_BODY_INFO_COMPLETED)
if (method != eCONNECT_GRAPHICS_SERVER && method != eCONNECT_GRAPHICS_SERVER_MAIN_THREAD)
{
printf("Connection terminated, couldn't get body info\n");
b3DisconnectSharedMemory(sm);
sm = 0;
sPhysicsClients1[freeIndex] = 0;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
return PyInt_FromLong(-1);
}
command = b3InitSyncBodyInfoCommand(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
command = b3InitSyncUserDataCommand(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SYNC_BODY_INFO_COMPLETED)
{
printf("Connection terminated, couldn't get body info\n");
b3DisconnectSharedMemory(sm);
sm = 0;
sPhysicsClients1[freeIndex] = 0;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
return PyInt_FromLong(-1);
}
if (statusType != CMD_SYNC_USER_DATA_COMPLETED)
{
printf("Connection terminated, couldn't get user data\n");
b3DisconnectSharedMemory(sm);
sm = 0;
sPhysicsClients1[freeIndex] = 0;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
return PyInt_FromLong(-1);
command = b3InitSyncUserDataCommand(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SYNC_USER_DATA_COMPLETED)
{
printf("Connection terminated, couldn't get user data\n");
b3DisconnectSharedMemory(sm);
sm = 0;
sPhysicsClients1[freeIndex] = 0;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
return PyInt_FromLong(-1);
}
}
}
}
@ -12568,6 +12597,12 @@ initpybullet(void)
PyModule_AddIntConstant(m, "GUI_MAIN_THREAD", eCONNECT_GUI_MAIN_THREAD); // user read
PyModule_AddIntConstant(m, "SHARED_MEMORY_SERVER", eCONNECT_SHARED_MEMORY_SERVER); // user read
PyModule_AddIntConstant(m, "SHARED_MEMORY_GUI", eCONNECT_SHARED_MEMORY_GUI); // user read
PyModule_AddIntConstant(m, "GRAPHICS_CLIENT", eCONNECT_SHARED_MEMORY_GUI); // user read
PyModule_AddIntConstant(m, "GRAPHICS_SERVER", eCONNECT_GRAPHICS_SERVER); // user read
PyModule_AddIntConstant(m, "GRAPHICS_SERVER_TCP", eCONNECT_GRAPHICS_SERVER_TCP); // user read
PyModule_AddIntConstant(m, "GRAPHICS_SERVER_MAIN_THREAD", eCONNECT_GRAPHICS_SERVER_MAIN_THREAD); // user read
#ifdef BT_ENABLE_DART

23
setup.py
View File

@ -101,6 +101,14 @@ else:
include_dirs += NP_DIRS
sources = ["examples/pybullet/pybullet.c"]\
+["examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp"]\
+["src/btLinearMathAll.cpp"]\
+["src/btBulletCollisionAll.cpp"]\
+["src/btBulletDynamicsAll.cpp"]\
@ -119,7 +127,9 @@ sources = ["examples/pybullet/pybullet.c"]\
+["examples/SharedMemory/PhysicsClient.cpp"]\
+["examples/SharedMemory/PhysicsServer.cpp"]\
+["examples/SharedMemory/GraphicsClientExample.cpp"]\
+["examples/SharedMemory/GraphicsServerExample.cpp"]\
+["examples/SharedMemory/RemoteGUIHelper.cpp"]\
+["examples/SharedMemory/RemoteGUIHelperTCP.cpp"]\
+["examples/SharedMemory/PhysicsServerExample.cpp"]\
+["examples/SharedMemory/PhysicsServerExampleBullet2.cpp"]\
+["examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp"]\
@ -134,13 +144,6 @@ sources = ["examples/pybullet/pybullet.c"]\
+["examples/SharedMemory/PosixSharedMemory.cpp"]\
+["examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp"]\
+["examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp"]\
+["examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp"]\
+["examples/SharedMemory/PhysicsClientUDP.cpp"]\
+["examples/SharedMemory/PhysicsClientUDP_C_API.cpp"]\
+["examples/SharedMemory/PhysicsClientTCP.cpp"]\
@ -151,7 +154,6 @@ sources = ["examples/pybullet/pybullet.c"]\
+["examples/Utils/ChromeTraceUtil.cpp"]\
+["examples/Utils/b3Clock.cpp"]\
+["examples/Utils/b3Quickprof.cpp"]\
+["examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp"]\
+["examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp"]\
+["examples/ThirdPartyLibs/stb_image/stb_image.cpp"]\
+["examples/ThirdPartyLibs/stb_image/stb_image_write.cpp"]\
@ -499,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
setup(
name='pybullet',
version='2.6.8',
version='2.7.0',
description=
'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description=
@ -513,9 +515,6 @@ setup(
'game development', 'virtual reality', 'physics simulation', 'robotics',
'collision detection', 'opengl'
],
install_requires=[
'numpy',
],
ext_modules=extensions,
classifiers=[
'Development Status :: 5 - Production/Stable',

29
test/SharedMemory/premake4.lua
View File

@ -370,6 +370,33 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
}
end
if not _OPTIONS["no-clsocket"] then
includedirs {"../../examples/ThirdPartyLibs/clsocket/src"}
if os.is("Windows") then
defines { "WIN32" }
links {"Ws2_32","Winmm"}
end
if os.is("Linux") then
defines {"_LINUX"}
end
if os.is("MacOSX") then
defines {"_DARWIN"}
end
links {"clsocket"}
files {
"../../examples/SharedMemory/PhysicsClientTCP.cpp",
"../../examples/SharedMemory/PhysicsClientTCP.h",
"../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp",
"../../examples/SharedMemory/PhysicsClientTCP_C_API.h",
}
defines {"BT_ENABLE_CLSOCKET"}
end
files {
"test.c",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
@ -380,6 +407,8 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/RemoteGUIHelper.cpp",
"../../examples/SharedMemory/RemoteGUIHelperTCP.cpp",
"../../examples/SharedMemory/GraphicsServerExample.cpp",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/InProcessMemory.cpp",
"../../examples/SharedMemory/PhysicsClient.cpp",