From 6ab09fe06e82815e91c88c5e27d2ade463d1500a Mon Sep 17 00:00:00 2001 From: bla Date: Thu, 6 Jun 2019 21:54:22 +0000 Subject: [PATCH 1/7] fix testlaikago and pd_controller_stable --- .../pybullet_envs/deep_mimic/env/testLaikago.py | 17 +++++++++++------ .../gym/pybullet_utils/pd_controller_stable.py | 2 +- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py index ef2b21c33..4a505cd7e 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py @@ -56,7 +56,7 @@ for i in range(4): jointOffsets.append(-0.7) jointOffsets.append(0.7) -maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20) +maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 120) for j in range(p.getNumJoints(quadruped)): p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0) @@ -103,7 +103,7 @@ for i in range(4): jointOffsets.append(-0.7) jointOffsets.append(0.7) -maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20) +maxUpForceId = p.addUserDebugParameter("maxUpForce", 0, 100, 20) for j in range(p.getNumJoints(quadruped)): p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0) @@ -162,7 +162,11 @@ while t < 10. * cycleTime: jointsStr, qdot = qpi.Slerp(frameFraction, frameData, frameDataNext, p) maxForce = p.readUserDebugParameter(maxForceId) - print("jointIds=", jointIds) + #print("jointIds=", jointIds) + + maxUpForce = p.readUserDebugParameter(maxUpForceId) + p.changeConstraint(cid, maxForce=maxUpForce) + if useConstraints: for j in range(12): @@ -194,7 +198,7 @@ while t < 10. * cycleTime: desiredVelocities=desiredVelocities, kps=[4000] * totalDofs, kds=[40] * totalDofs, - maxForces=[500] * totalDofs, + maxForces=[maxForce] * totalDofs, timeStep=timeStep) dofIndex = 6 @@ -202,7 +206,7 @@ while t < 10. * cycleTime: for index in range(len(jointIds)): jointIndex = jointIds[index] force = [scaling * taus[dofIndex]] - print("force[", jointIndex, "]=", force) + #print("force[", jointIndex, "]=", force) p.setJointMotorControlMultiDof(quadruped, jointIndex, controlMode=p.TORQUE_CONTROL, @@ -231,8 +235,9 @@ if useOrgData: force=maxForce) p.stepSimulation() for lower_leg in lower_legs: + pass #print("points for ", quadruped, " link: ", lower_leg) - pts = p.getContactPoints(quadruped, -1, lower_leg) + #pts = p.getContactPoints(quadruped, -1, lower_leg) #print("num points=",len(pts)) #for pt in pts: # print(pt[9]) diff --git a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py index 87b68d462..58854027b 100644 --- a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py +++ b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py @@ -227,6 +227,6 @@ class PDControllerStable(object): qddot = np.linalg.solve(A, b) tau = p + d - Kd.dot(qddot) * timeStep maxF = np.array(maxForces) - forces = np.clip(tau, -maxF, maxF) + tau = np.clip(tau, -maxF, maxF) #print("c=",c) return tau From 8f9fac99bddeba976269bb571530804ca26716ad Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 13 Jun 2019 13:26:52 -0700 Subject: [PATCH 2/7] add kinematic version that also updates the base position/orientation from the base --- .../deep_mimic/env/testLaikago.py | 100 ++++++++++-------- 1 file changed, 56 insertions(+), 44 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py index 4a505cd7e..faee61fda 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py @@ -7,6 +7,7 @@ import time import motion_capture_data import quadrupedPoseInterpolator +useKinematic = False useConstraints = False p = bullet_client.BulletClient(connection_mode=p1.GUI) @@ -168,52 +169,63 @@ while t < 10. * cycleTime: p.changeConstraint(cid, maxForce=maxUpForce) - if useConstraints: + if useKinematic: + basePos = startPos + basePos = [float(-jointsStr[0]),-float(jointsStr[1]),float(jointsStr[2])] + baseOrn = [float(jointsStr[4]),float(jointsStr[5]),float(jointsStr[6]), float(jointsStr[3])] + p.resetBasePositionAndOrientation(quadruped, basePos,baseOrn) + for j in range(12): - #skip the base positional dofs - targetPos = float(jointsStr[j + 7]) - p.setJointMotorControl2(quadruped, - jointIds[j], - p.POSITION_CONTROL, - jointDirections[j] * targetPos + jointOffsets[j], - force=maxForce) - + #skip the base positional dofs + targetPos = float(jointsStr[j + 7]) + p.resetJointState(quadruped,jointIds[j],jointDirections[j] * targetPos + jointOffsets[j]) else: - desiredPositions = [] - for j in range(7): - targetPosUnmodified = float(jointsStr[j]) - desiredPositions.append(targetPosUnmodified) - for j in range(12): - targetPosUnmodified = float(jointsStr[j + 7]) - targetPos = jointDirections[j] * targetPosUnmodified + jointOffsets[j] - desiredPositions.append(targetPos) - numBaseDofs = 6 - totalDofs = 12 + numBaseDofs - desiredVelocities = None - if desiredVelocities == None: - desiredVelocities = [0] * totalDofs - taus = stablePD.computePD(bodyUniqueId=quadruped, - jointIndices=jointIds, - desiredPositions=desiredPositions, - desiredVelocities=desiredVelocities, - kps=[4000] * totalDofs, - kds=[40] * totalDofs, - maxForces=[maxForce] * totalDofs, - timeStep=timeStep) - - dofIndex = 6 - scaling = 1 - for index in range(len(jointIds)): - jointIndex = jointIds[index] - force = [scaling * taus[dofIndex]] - #print("force[", jointIndex, "]=", force) - p.setJointMotorControlMultiDof(quadruped, - jointIndex, - controlMode=p.TORQUE_CONTROL, - force=force) - dofIndex += 1 - - p.stepSimulation() + if useConstraints: + for j in range(12): + #skip the base positional dofs + targetPos = float(jointsStr[j + 7]) + p.setJointMotorControl2(quadruped, + jointIds[j], + p.POSITION_CONTROL, + jointDirections[j] * targetPos + jointOffsets[j], + force=maxForce) + + else: + desiredPositions = [] + for j in range(7): + targetPosUnmodified = float(jointsStr[j]) + desiredPositions.append(targetPosUnmodified) + for j in range(12): + targetPosUnmodified = float(jointsStr[j + 7]) + targetPos = jointDirections[j] * targetPosUnmodified + jointOffsets[j] + desiredPositions.append(targetPos) + numBaseDofs = 6 + totalDofs = 12 + numBaseDofs + desiredVelocities = None + if desiredVelocities == None: + desiredVelocities = [0] * totalDofs + taus = stablePD.computePD(bodyUniqueId=quadruped, + jointIndices=jointIds, + desiredPositions=desiredPositions, + desiredVelocities=desiredVelocities, + kps=[4000] * totalDofs, + kds=[40] * totalDofs, + maxForces=[maxForce] * totalDofs, + timeStep=timeStep) + + dofIndex = 6 + scaling = 1 + for index in range(len(jointIds)): + jointIndex = jointIds[index] + force = [scaling * taus[dofIndex]] + #print("force[", jointIndex, "]=", force) + p.setJointMotorControlMultiDof(quadruped, + jointIndex, + controlMode=p.TORQUE_CONTROL, + force=force) + dofIndex += 1 + + p.stepSimulation() t += timeStep time.sleep(timeStep) From 6160b52fd7813ce43646b67c178a5b978edf6a00 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 13 Jun 2019 23:24:22 -0700 Subject: [PATCH 3/7] graphicsServer to workaround OpenGL issues on some servers. --- .../CommonGUIHelperInterface.h | 14 +- examples/ExampleBrowser/ExampleEntries.cpp | 8 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 37 +- .../OpenGLWindow/GLInstancingRenderer.cpp | 3 +- examples/SharedMemory/CMakeLists.txt | 7 + .../SharedMemory/GraphicsClientExample.cpp | 287 +++++++++++++ examples/SharedMemory/GraphicsClientExample.h | 7 + .../SharedMemory/GraphicsServerExample.cpp | 398 ++++++++++++++++++ examples/SharedMemory/GraphicsServerExample.h | 6 + .../SharedMemory/GraphicsSharedMemoryBlock.h | 41 ++ .../GraphicsSharedMemoryCommands.h | 159 +++++++ .../SharedMemory/GraphicsSharedMemoryPublic.h | 47 +++ .../PhysicsServerCommandProcessor.cpp | 1 + .../SharedMemory/PhysicsServerExample.cpp | 40 ++ .../PhysicsServerSharedMemory.cpp | 2 + examples/SharedMemory/SharedMemoryBlock.h | 2 +- .../SharedMemoryInProcessPhysicsC_API.cpp | 21 + .../SharedMemoryInProcessPhysicsC_API.h | 4 +- examples/SharedMemory/SharedMemoryPublic.h | 1 + examples/SharedMemory/premake4.lua | 7 + .../pybullet/examples/inverse_kinematics.py | 9 +- examples/pybullet/pybullet.c | 11 + 22 files changed, 1091 insertions(+), 21 deletions(-) create mode 100644 examples/SharedMemory/GraphicsClientExample.cpp create mode 100644 examples/SharedMemory/GraphicsClientExample.h create mode 100644 examples/SharedMemory/GraphicsServerExample.cpp create mode 100644 examples/SharedMemory/GraphicsServerExample.h create mode 100644 examples/SharedMemory/GraphicsSharedMemoryBlock.h create mode 100644 examples/SharedMemory/GraphicsSharedMemoryCommands.h create mode 100644 examples/SharedMemory/GraphicsSharedMemoryPublic.h diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 5e3478ec7..fd56f879e 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -11,6 +11,13 @@ struct CommonParameterInterface; struct CommonRenderInterface; struct CommonGraphicsApp; +struct GUISyncPosition +{ + int m_graphicsInstanceId; + float m_pos[4]; + float m_orn[4]; +}; + typedef void (*VisualizerFlagCallback)(int flag, bool enable); ///The Bullet 2 GraphicsPhysicsBridge let's the graphics engine create graphics representation and synchronize @@ -26,6 +33,8 @@ struct GUIHelperInterface virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) = 0; + virtual void syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions) {} + virtual void render(const btDiscreteDynamicsWorld* rbWorld) = 0; virtual void createPhysicsDebugDrawer(btDiscreteDynamicsWorld* rbWorld) = 0; @@ -114,7 +123,10 @@ struct GUIHelperInterface ///the DummyGUIHelper does nothing, so we can test the examples without GUI/graphics (in 'console mode') struct DummyGUIHelper : public GUIHelperInterface { - DummyGUIHelper() {} + DummyGUIHelper() + { + + } virtual ~DummyGUIHelper() {} virtual void createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color) {} diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index c21fa8618..e80b4eee9 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -11,6 +11,9 @@ #include "../RenderingExamples/RaytracerSetup.h" #include "../RenderingExamples/TinyRendererSetup.h" #include "../RenderingExamples/DynamicTexturedCubeDemo.h" +#include "../SharedMemory/GraphicsServerExample.h" +#include "../SharedMemory/GraphicsClientExample.h" + #include "../ForkLift/ForkLiftDemo.h" #include "../MultiThreadedDemo/MultiThreadedDemo.h" #include "../BasicDemo/BasicExample.h" @@ -150,7 +153,10 @@ static ExampleEntry gDefaultExamples[] = PhysicsServerCreateFuncBullet2, PHYSICS_SERVER_ENABLE_COMMAND_LOGGING), ExampleEntry(1, "Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.", PhysicsServerCreateFuncBullet2, PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG), - // + ExampleEntry(1, "Graphics Server", "Create a graphics server.",GraphicsServerCreateFuncBullet), + ExampleEntry(1, "Graphics Client", "Create a graphics client.", GraphicsClientCreateFunc), + + // // ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT), ExampleEntry(0, "BlockSolver"), diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index c7a2e8a69..d60390d60 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -1157,17 +1157,22 @@ void OpenGLExampleBrowser::updateGraphics() void OpenGLExampleBrowser::update(float deltaTime) { + b3ChromeUtilsEnableProfiling(); if (!gEnableRenderLoop && !singleStepSimulation) { + B3_PROFILE("updateGraphics"); sCurrentDemo->updateGraphics(); return; } B3_PROFILE("OpenGLExampleBrowser::update"); - assert(glGetError() == GL_NO_ERROR); - s_instancingRenderer->init(); + //assert(glGetError() == GL_NO_ERROR); + { + B3_PROFILE("s_instancingRenderer"); + s_instancingRenderer->init(); + } DrawGridData dg; dg.upAxis = s_app->getUpAxis(); @@ -1215,6 +1220,7 @@ void OpenGLExampleBrowser::update(float deltaTime) if (gFixedTimeStep > 0) { + sCurrentDemo->stepSimulation(gFixedTimeStep); } else @@ -1279,22 +1285,25 @@ void OpenGLExampleBrowser::update(float deltaTime) } #endif //#ifndef BT_NO_PROFILE - if (sUseOpenGL2) { - saveOpenGLState(s_instancingRenderer->getScreenWidth() * s_window->getRetinaScale(), s_instancingRenderer->getScreenHeight() * s_window->getRetinaScale()); - } + B3_PROFILE("updateOpenGL"); + if (sUseOpenGL2) + { + saveOpenGLState(s_instancingRenderer->getScreenWidth() * s_window->getRetinaScale(), s_instancingRenderer->getScreenHeight() * s_window->getRetinaScale()); + } - if (m_internalData->m_gui) - { - gBlockGuiMessages = true; - m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight()); + if (m_internalData->m_gui) + { + gBlockGuiMessages = true; + m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight()); - gBlockGuiMessages = false; - } + gBlockGuiMessages = false; + } - if (sUseOpenGL2) - { - restoreOpenGLState(); + if (sUseOpenGL2) + { + restoreOpenGLState(); + } } } diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index a35f3a1ad..ef8bc68c1 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -2010,6 +2010,7 @@ TransparentDistanceSortPredicate{ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode) { + B3_PROFILE("renderSceneInternal"); int renderMode = orgRenderMode; bool reflectionPass = false; bool reflectionPlanePass = false; @@ -2047,7 +2048,7 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode) // Cull triangles which normal is not towards the camera glEnable(GL_CULL_FACE); - B3_PROFILE("GLInstancingRenderer::RenderScene"); + { B3_PROFILE("init"); diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index 84c154fa0..93a8fc6b9 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -18,6 +18,13 @@ SET(SharedMemory_SRCS PhysicsServer.cpp PhysicsServer.h PhysicsClientC_API.cpp + GraphicsClientExample.cpp + GraphicsClientExample.h + GraphicsServerExample.cpp + GraphicsServerExample.h + GraphicsSharedMemoryBlock.h + GraphicsSharedMemoryCommands.h + GraphicsSharedMemoryPublic.h SharedMemoryCommands.h SharedMemoryPublic.h PhysicsServer.cpp diff --git a/examples/SharedMemory/GraphicsClientExample.cpp b/examples/SharedMemory/GraphicsClientExample.cpp new file mode 100644 index 000000000..7db782559 --- /dev/null +++ b/examples/SharedMemory/GraphicsClientExample.cpp @@ -0,0 +1,287 @@ + +#include "GraphicsClientExample.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "Bullet3Common/b3Logging.h" +#include "GraphicsSharedMemoryCommands.h" +#include "PosixSharedMemory.h" +#include "Win32SharedMemory.h" +#include "GraphicsSharedMemoryBlock.h" +#include "Bullet3Common/b3Scalar.h" + +class GraphicsClientExample : public CommonExampleInterface +{ +protected: + + GUIHelperInterface* m_guiHelper; + bool m_waitingForServer; + GraphicsSharedMemoryBlock* m_testBlock1; + SharedMemoryInterface* m_sharedMemory; + GraphicsSharedMemoryStatus m_lastServerStatus; + int m_sharedMemoryKey; + bool m_isConnected; + +public: + GraphicsClientExample(GUIHelperInterface* helper, int options); + virtual ~GraphicsClientExample(); + + virtual void initPhysics(); + virtual void stepSimulation(float deltaTime); + + virtual void resetCamera() + { + float dist = 3.45; + float pitch = -16.2; + float yaw = 287; + float targetPos[3] = {2.05, 0.02, 0.53}; //-3,2.8,-2.5}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + + virtual bool isConnected() + { + return m_isConnected; + } + + bool canSubmitCommand() const + { + if (m_isConnected && !m_waitingForServer) + { + if (m_testBlock1->m_magicId == GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER) + { + return true; + } + else + { + return false; + } + } + return false; + } + + struct GraphicsSharedMemoryCommand* getAvailableSharedMemoryCommand() + { + static int sequence = 0; + if (m_testBlock1) + { + m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++; + return &m_testBlock1->m_clientCommands[0]; + } + return 0; + } + + bool submitClientCommand(const GraphicsSharedMemoryCommand& 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 + + if (!m_waitingForServer) + { + //printf("submit command of type %d\n", command.m_type); + + if (&m_testBlock1->m_clientCommands[0] != &command) + { + m_testBlock1->m_clientCommands[0] = command; + } + m_testBlock1->m_numClientCommands++; + m_waitingForServer = true; + return true; + } + return false; + } + + + const GraphicsSharedMemoryStatus* processServerStatus() + { + // SharedMemoryStatus* stat = 0; + + if (!m_testBlock1) + { + m_lastServerStatus.m_type = GFX_CMD_SHARED_MEMORY_NOT_INITIALIZED; + return &m_lastServerStatus; + } + + if (!m_waitingForServer) + { + return 0; + } + + if (m_testBlock1->m_magicId != GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER) + { + m_lastServerStatus.m_type = GFX_CMD_SHARED_MEMORY_NOT_INITIALIZED; + return &m_lastServerStatus; + } + + + if (m_testBlock1->m_numServerCommands > + m_testBlock1->m_numProcessedServerCommands) + { + B3_PROFILE("processServerCMD"); + b3Assert(m_testBlock1->m_numServerCommands == + m_testBlock1->m_numProcessedServerCommands + 1); + + const GraphicsSharedMemoryStatus& serverCmd = m_testBlock1->m_serverCommands[0]; + + m_lastServerStatus = serverCmd; + + // EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; + // consume the command + switch (serverCmd.m_type) + { + case GFX_CMD_CLIENT_COMMAND_COMPLETED: + { + B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED"); + + + break; + } + default: + { + } + } + + m_testBlock1->m_numProcessedServerCommands++; + // we don't have more than 1 command outstanding (in total, either server or client) + b3Assert(m_testBlock1->m_numProcessedServerCommands == + m_testBlock1->m_numServerCommands); + + if (m_testBlock1->m_numServerCommands == + m_testBlock1->m_numProcessedServerCommands) + { + m_waitingForServer = false; + } + else + { + m_waitingForServer = true; + } + + + return &m_lastServerStatus; + } + return 0; + } + + bool connect() + { + /// server always has to create and initialize shared memory + bool allowCreation = false; + m_testBlock1 = (GraphicsSharedMemoryBlock*)m_sharedMemory->allocateSharedMemory( + m_sharedMemoryKey, GRAPHICS_SHARED_MEMORY_SIZE, allowCreation); + + if (m_testBlock1) + { + if (m_testBlock1->m_magicId != GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER) + { + b3Error("Error connecting to shared memory: please start server before client\n"); + m_sharedMemory->releaseSharedMemory(m_sharedMemoryKey, + GRAPHICS_SHARED_MEMORY_SIZE); + m_testBlock1 = 0; + return false; + } + else + { + m_isConnected = true; + } + } + else + { + b3Warning("Cannot connect to shared memory"); + return false; + } + return true; + } + + + void disconnect() + { + if (m_isConnected && m_sharedMemory) + { + m_sharedMemory->releaseSharedMemory(m_sharedMemoryKey, GRAPHICS_SHARED_MEMORY_SIZE); + } + m_isConnected = false; + } + + virtual void exitPhysics(){}; + + virtual void physicsDebugDraw(int debugFlags) + { + } + + virtual void renderScene() + { + } + virtual bool mouseMoveCallback(float x, float y) + { + return false; + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return false; + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + +}; + + + +GraphicsClientExample::GraphicsClientExample(GUIHelperInterface* helper, int options) + : m_guiHelper(helper), + m_waitingForServer(false), + m_testBlock1(0) +{ +#ifdef _WIN32 + m_sharedMemory = new Win32SharedMemoryClient(); +#else + m_sharedMemory = new PosixSharedMemory(); +#endif + m_sharedMemoryKey = GRAPHICS_SHARED_MEMORY_KEY; + m_isConnected = false; + b3Printf("Started GraphicsClientExample\n"); + connect(); +} + +GraphicsClientExample::~GraphicsClientExample() +{ + disconnect(); + delete m_sharedMemory; +} + + +void GraphicsClientExample::initPhysics() +{ + if (m_guiHelper && m_guiHelper->getParameterInterface()) + { + int upAxis = 2; + m_guiHelper->setUpAxis(upAxis); + } + +} + +void GraphicsClientExample::stepSimulation(float deltaTime) +{ + GraphicsSharedMemoryCommand* cmd = getAvailableSharedMemoryCommand(); + if (cmd) + { + cmd->m_updateFlags = 0; + cmd->m_type = GFX_CMD_0; + submitClientCommand(*cmd); + } + const GraphicsSharedMemoryStatus* status = processServerStatus(); + if (status) + { + //handle it + } +} + +class CommonExampleInterface* GraphicsClientCreateFunc(struct CommonExampleOptions& options) +{ + GraphicsClientExample* example = new GraphicsClientExample(options.m_guiHelper, options.m_option); + + + return example; +} diff --git a/examples/SharedMemory/GraphicsClientExample.h b/examples/SharedMemory/GraphicsClientExample.h new file mode 100644 index 000000000..403fa31dd --- /dev/null +++ b/examples/SharedMemory/GraphicsClientExample.h @@ -0,0 +1,7 @@ +#ifndef GRAPHICS_CLIENT_EXAMPLE_H +#define GRAPHICS_CLIENT_EXAMPLE_H + + +class CommonExampleInterface* GraphicsClientCreateFunc(struct CommonExampleOptions& options); + +#endif //GRAPHICS_CLIENT_EXAMPLE_H diff --git a/examples/SharedMemory/GraphicsServerExample.cpp b/examples/SharedMemory/GraphicsServerExample.cpp new file mode 100644 index 000000000..944a363d1 --- /dev/null +++ b/examples/SharedMemory/GraphicsServerExample.cpp @@ -0,0 +1,398 @@ + +#include "GraphicsServerExample.h" +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "PosixSharedMemory.h" +#include "Win32SharedMemory.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "LinearMath/btTransform.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "GraphicsSharedMemoryBlock.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" + +#define MAX_GRAPHICS_SHARED_MEMORY_BLOCKS 1 + + +class GraphicsServerExample : public CommonExampleInterface +{ + CommonGraphicsApp* m_app; + GUIHelperInterface* m_guiHelper; + SharedMemoryInterface* m_sharedMemory; + int m_sharedMemoryKey; + bool m_verboseOutput; + bool m_areConnected[MAX_GRAPHICS_SHARED_MEMORY_BLOCKS]; + GraphicsSharedMemoryBlock* m_testBlocks[MAX_GRAPHICS_SHARED_MEMORY_BLOCKS]; + b3AlignedObjectArray< b3AlignedObjectArray > m_dataSlots; + + float m_x; + float m_y; + float m_z; + +public: + GraphicsServerExample(GUIHelperInterface* guiHelper) + : m_guiHelper(guiHelper), + m_x(0), + m_y(0), + m_z(0) + { + m_verboseOutput = true; + m_sharedMemoryKey = GRAPHICS_SHARED_MEMORY_KEY; + m_app = guiHelper->getAppInterface(); + m_app->setUpAxis(2); + + + for (int i = 0; i < MAX_GRAPHICS_SHARED_MEMORY_BLOCKS; i++) + { + m_areConnected[i] = false; + } + +#ifdef _WIN32 + m_sharedMemory = new Win32SharedMemoryServer(); +#else + m_sharedMemory = new PosixSharedMemory(); +#endif + +#if 0 + { + int boxId = m_app->registerCubeShape(0.1, 0.1, 0.1); + btVector3 pos(0, 0, 0); + btQuaternion orn(0, 0, 0, 1); + btVector4 color(0.3, 0.3, 0.3, 1); + btVector3 scaling(1, 1, 1); + m_app->m_renderer->registerGraphicsInstance(boxId, pos, orn, color, scaling); + } + + m_app->m_renderer->writeTransforms(); +#endif + connectSharedMemory(m_guiHelper, m_sharedMemoryKey); + + } + virtual ~GraphicsServerExample() + { + disconnectSharedMemory(); + } + + virtual void initPhysics() + { + } + virtual void exitPhysics() + { + } + + GraphicsSharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp, int blockIndex) + { + GraphicsSharedMemoryStatus& serverCmd = m_testBlocks[blockIndex]->m_serverCommands[0]; + serverCmd.m_type = statusType; + serverCmd.m_sequenceNumber = sequenceNumber; + serverCmd.m_timeStamp = timeStamp; + return serverCmd; + } + void submitServerStatus(GraphicsSharedMemoryStatus& status, int blockIndex) + { + m_testBlocks[blockIndex]->m_numServerCommands++; + } + + bool processCommand(const struct GraphicsSharedMemoryCommand& clientCmd, struct GraphicsSharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) + { + //printf("processed command of type:%d\n", clientCmd.m_type); + B3_PROFILE("processCommand"); + switch (clientCmd.m_type) + { + case GFX_CMD_0: + { + //either Y or Z can be up axis + int upAxis = (clientCmd.m_upAxisYCommand.m_enableUpAxisY) ? 1 : 2; + m_guiHelper->setUpAxis(upAxis); + break; + } + case GFX_CMD_SET_VISUALIZER_FLAG: + { + //printf("clientCmd.m_visualizerFlag.m_visualizerFlag: %d, clientCmd.m_visualizerFlag.m_enable %d\n", + // clientCmd.m_visualizerFlagCommand.m_visualizerFlag, clientCmd.m_visualizerFlagCommand.m_enable); + + this->m_guiHelper->setVisualizerFlag(clientCmd.m_visualizerFlagCommand.m_visualizerFlag, clientCmd.m_visualizerFlagCommand.m_enable); + break; + } + + case GFX_CMD_UPLOAD_DATA: + { + //printf("uploadData command: curSize=%d, offset=%d, slot=%d", clientCmd.m_uploadDataCommand.m_numBytes, clientCmd.m_uploadDataCommand.m_dataOffset, clientCmd.m_uploadDataCommand.m_dataSlot); + int dataSlot = clientCmd.m_uploadDataCommand.m_dataSlot; + int dataOffset = clientCmd.m_uploadDataCommand.m_dataOffset; + m_dataSlots.resize(dataSlot + 1); + btAssert(m_dataSlots[dataSlot].size() >= dataOffset); + m_dataSlots[dataSlot].resize(clientCmd.m_uploadDataCommand.m_numBytes + clientCmd.m_uploadDataCommand.m_dataOffset); + for (int i = 0; i < clientCmd.m_uploadDataCommand.m_numBytes; i++) + { + m_dataSlots[dataSlot][dataOffset + i] = bufferServerToClient[i]; + } + break; + } + case GFX_CMD_REGISTER_TEXTURE: + { + int dataSlot = 0; + int sizeData = m_dataSlots[dataSlot].size(); + btAssert(sizeData > 0); + serverStatusOut.m_type = GFX_CMD_REGISTER_TEXTURE_FAILED; + if (sizeData) + { + unsigned char* texels = &m_dataSlots[dataSlot][0]; + int textureId = this->m_guiHelper->registerTexture(texels, clientCmd.m_registerTextureCommand.m_width, clientCmd.m_registerTextureCommand.m_height); + serverStatusOut.m_type = GFX_CMD_REGISTER_TEXTURE_COMPLETED; + serverStatusOut.m_registerTextureStatus.m_textureId = textureId; + } + break; + } + + case GFX_CMD_REGISTER_GRAPHICS_SHAPE: + { + int verticesSlot = 0; + int indicesSlot = 1; + serverStatusOut.m_type = GFX_CMD_REGISTER_GRAPHICS_SHAPE_FAILED; + const float* vertices = (const float*) &m_dataSlots[verticesSlot][0]; + const int* indices = (const int*) &m_dataSlots[indicesSlot][0]; + int numVertices = clientCmd.m_registerGraphicsShapeCommand.m_numVertices; + int numIndices = clientCmd.m_registerGraphicsShapeCommand.m_numIndices; + int primitiveType = clientCmd.m_registerGraphicsShapeCommand.m_primitiveType; + int textureId = clientCmd.m_registerGraphicsShapeCommand.m_textureId; + int shapeId = this->m_guiHelper->registerGraphicsShape(vertices, numVertices, indices, numIndices, primitiveType, textureId); + serverStatusOut.m_registerGraphicsShapeStatus.m_shapeId = shapeId; + serverStatusOut.m_type = GFX_CMD_REGISTER_GRAPHICS_SHAPE_COMPLETED; + break; + } + + case GFX_CMD_REGISTER_GRAPHICS_INSTANCE: + { + int graphicsInstanceId = m_guiHelper->registerGraphicsInstance(clientCmd.m_registerGraphicsInstanceCommand.m_shapeIndex, + clientCmd.m_registerGraphicsInstanceCommand.m_position, + clientCmd.m_registerGraphicsInstanceCommand.m_quaternion, + clientCmd.m_registerGraphicsInstanceCommand.m_color, + clientCmd.m_registerGraphicsInstanceCommand.m_scaling); + serverStatusOut.m_registerGraphicsInstanceStatus.m_graphicsInstanceId = graphicsInstanceId; + serverStatusOut.m_type = GFX_CMD_REGISTER_GRAPHICS_INSTANCE_COMPLETED; + + break; + } + case GFX_CMD_SYNCHRONIZE_TRANSFORMS: + { + GUISyncPosition* positions = (GUISyncPosition*)bufferServerToClient; + for (int i = 0; i < clientCmd.m_syncTransformsCommand.m_numPositions; i++) + { + m_app->m_renderer->writeSingleInstanceTransformToCPU(positions[i].m_pos, positions[i].m_orn, positions[i].m_graphicsInstanceId); + } + break; + } + case GFX_CMD_REMOVE_ALL_GRAPHICS_INSTANCES: + { + m_guiHelper->removeAllGraphicsInstances(); + break; + } + case GFX_CMD_REMOVE_SINGLE_GRAPHICS_INSTANCE: + { + m_app->m_renderer->removeGraphicsInstance(clientCmd.m_removeGraphicsInstanceCommand.m_graphicsUid); + break; + } + default: + { + } + } + return true; + } + + void processClientCommands() + { + B3_PROFILE("processClientCommands"); + for (int block = 0; block < MAX_GRAPHICS_SHARED_MEMORY_BLOCKS; block++) + { + if (m_areConnected[block] && m_testBlocks[block]) + { + ///we ignore overflow of integer for now + if (m_testBlocks[block]->m_numClientCommands > m_testBlocks[block]->m_numProcessedClientCommands) + { + //BT_PROFILE("processClientCommand"); + + //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands + btAssert(m_testBlocks[block]->m_numClientCommands == m_testBlocks[block]->m_numProcessedClientCommands + 1); + + const GraphicsSharedMemoryCommand& clientCmd = m_testBlocks[block]->m_clientCommands[0]; + + m_testBlocks[block]->m_numProcessedClientCommands++; + //todo, timeStamp + int timeStamp = 0; + GraphicsSharedMemoryStatus& serverStatusOut = createServerStatus(GFX_CMD_CLIENT_COMMAND_FAILED, clientCmd.m_sequenceNumber, timeStamp, block); + bool hasStatus = processCommand(clientCmd, serverStatusOut, &m_testBlocks[block]->m_bulletStreamData[0], GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + submitServerStatus(serverStatusOut, block); + } + } + } + } + } + + virtual void stepSimulation(float deltaTime) + { + B3_PROFILE("stepSimulation"); + processClientCommands(); + m_x += 0.01f; + m_y += 0.01f; + m_z += 0.01f; + } + + + virtual void renderScene() + { + B3_PROFILE("renderScene"); + { + + B3_PROFILE("writeTransforms"); + m_app->m_renderer->writeTransforms(); + } + { + B3_PROFILE("m_renderer->renderScene"); + m_app->m_renderer->renderScene(); + } + + } + + + + virtual void physicsDebugDraw(int debugDrawFlags) + { + + } + + virtual bool mouseMoveCallback(float x, float y) + { + return false; + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return false; + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 3.5; + float pitch = -32; + float yaw = 136; + float targetPos[3] = {0, 0, 0}; + if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) + { + m_app->m_renderer->getActiveCamera()->setCameraDistance(dist); + m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch); + m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw); + m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0], targetPos[1], targetPos[2]); + } + } + + bool connectSharedMemory(struct GUIHelperInterface* guiHelper, int sharedMemoryKey); + void disconnectSharedMemory(bool deInitializeSharedMemory=true); +}; + + + +bool GraphicsServerExample::connectSharedMemory(struct GUIHelperInterface* guiHelper, int sharedMemoryKey) +{ + + bool allowCreation = true; + bool allConnected = false; + int numConnected = 0; + + int counter = 0; + for (int block = 0; block < MAX_GRAPHICS_SHARED_MEMORY_BLOCKS; block++) + { + if (m_areConnected[block]) + { + allConnected = true; + numConnected++; + b3Warning("connectSharedMemory, while already connected"); + continue; + } + do + { + m_testBlocks[block] = (GraphicsSharedMemoryBlock*)m_sharedMemory->allocateSharedMemory(m_sharedMemoryKey + block, GRAPHICS_SHARED_MEMORY_SIZE, allowCreation); + if (m_testBlocks[block]) + { + int magicId = m_testBlocks[block]->m_magicId; + if (m_verboseOutput) + { + b3Printf("magicId = %d\n", magicId); + } + + if (m_testBlocks[block]->m_magicId != GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER) + { + InitSharedMemoryBlock(m_testBlocks[block]); + if (m_verboseOutput) + { + b3Printf("Created and initialized shared memory block\n"); + } + m_areConnected[block] = true; + numConnected++; + } + else + { + m_sharedMemory->releaseSharedMemory(m_sharedMemoryKey + block, GRAPHICS_SHARED_MEMORY_SIZE); + m_testBlocks[block] = 0; + m_areConnected[block] = false; + } + } + else + { + //b3Error("Cannot connect to shared memory"); + m_areConnected[block] = false; + } + } while (counter++ < 10 && !m_areConnected[block]); + if (!m_areConnected[block]) + { + b3Error("Server cannot connect to shared memory.\n"); + } + } + + allConnected = (numConnected == MAX_GRAPHICS_SHARED_MEMORY_BLOCKS); + + return allConnected; +} + +void GraphicsServerExample::disconnectSharedMemory(bool deInitializeSharedMemory) +{ + //m_data->m_commandProcessor->deleteDynamicsWorld(); + + + if (m_verboseOutput) + { + b3Printf("releaseSharedMemory1\n"); + } + for (int block = 0; block < MAX_GRAPHICS_SHARED_MEMORY_BLOCKS; block++) + { + if (m_testBlocks[block]) + { + if (m_verboseOutput) + { + b3Printf("m_testBlock1\n"); + } + if (deInitializeSharedMemory) + { + m_testBlocks[block]->m_magicId = 0; + if (m_verboseOutput) + { + b3Printf("De-initialized shared memory, magic id = %d\n", m_testBlocks[block]->m_magicId); + } + } + btAssert(m_sharedMemory); + m_sharedMemory->releaseSharedMemory(m_sharedMemoryKey + block, GRAPHICS_SHARED_MEMORY_SIZE); + } + m_testBlocks[block] = 0; + m_areConnected[block] = false; + } +} + +CommonExampleInterface* GraphicsServerCreateFuncBullet(struct CommonExampleOptions& options) +{ + return new GraphicsServerExample(options.m_guiHelper); +} diff --git a/examples/SharedMemory/GraphicsServerExample.h b/examples/SharedMemory/GraphicsServerExample.h new file mode 100644 index 000000000..ac459675f --- /dev/null +++ b/examples/SharedMemory/GraphicsServerExample.h @@ -0,0 +1,6 @@ +#ifndef GRAPHICS_SERVER_EXAMPLE_H +#define GRAPHICS_SERVER_EXAMPLE_H + +class CommonExampleInterface* GraphicsServerCreateFuncBullet(struct CommonExampleOptions& options); + +#endif //GRAPHICS_SERVER_EXAMPLE_H diff --git a/examples/SharedMemory/GraphicsSharedMemoryBlock.h b/examples/SharedMemory/GraphicsSharedMemoryBlock.h new file mode 100644 index 000000000..108919644 --- /dev/null +++ b/examples/SharedMemory/GraphicsSharedMemoryBlock.h @@ -0,0 +1,41 @@ +#ifndef GRAPHICS_SHARED_MEMORY_BLOCK_H +#define GRAPHICS_SHARED_MEMORY_BLOCK_H + +#define GRAPHICS_SHARED_MEMORY_MAX_COMMANDS 1 + +#include "GraphicsSharedMemoryCommands.h" + +struct GraphicsSharedMemoryBlock +{ + int m_magicId; + struct GraphicsSharedMemoryCommand m_clientCommands[GRAPHICS_SHARED_MEMORY_MAX_COMMANDS]; + struct GraphicsSharedMemoryStatus m_serverCommands[GRAPHICS_SHARED_MEMORY_MAX_COMMANDS]; + + int m_numClientCommands; + int m_numProcessedClientCommands; + + int m_numServerCommands; + int m_numProcessedServerCommands; + + char m_bulletStreamData[GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; +}; + +//http://stackoverflow.com/questions/24736304/unable-to-use-inline-in-declaration-get-error-c2054 +#ifdef _WIN32 +__inline +#else +inline +#endif + void + InitSharedMemoryBlock(struct GraphicsSharedMemoryBlock* sharedMemoryBlock) +{ + sharedMemoryBlock->m_numClientCommands = 0; + sharedMemoryBlock->m_numServerCommands = 0; + sharedMemoryBlock->m_numProcessedClientCommands = 0; + sharedMemoryBlock->m_numProcessedServerCommands = 0; + sharedMemoryBlock->m_magicId = GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER; +} + +#define GRAPHICS_SHARED_MEMORY_SIZE sizeof(GraphicsSharedMemoryBlock) + +#endif //GRAPHICS_SHARED_MEMORY_BLOCK_H diff --git a/examples/SharedMemory/GraphicsSharedMemoryCommands.h b/examples/SharedMemory/GraphicsSharedMemoryCommands.h new file mode 100644 index 000000000..7c06c84a5 --- /dev/null +++ b/examples/SharedMemory/GraphicsSharedMemoryCommands.h @@ -0,0 +1,159 @@ +#ifndef GRAPHICS_SHARED_MEMORY_COMMANDS_H +#define GRAPHICS_SHARED_MEMORY_COMMANDS_H + +//this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc) + +#include "GraphicsSharedMemoryPublic.h" + +#ifdef __GNUC__ +#include +typedef int32_t smInt32a_t; +typedef int64_t smInt64a_t; +typedef uint32_t smUint32a_t; +typedef uint64_t smUint64a_t; +#elif defined(_MSC_VER) +typedef __int32 smInt32a_t; +typedef __int64 smInt64a_t; +typedef unsigned __int32 smUint32a_t; +typedef unsigned __int64 smUint64a_t; +#else +typedef int smInt32a_t; +typedef long long int smInt64a_t; +typedef unsigned int smUint32a_t; +typedef unsigned long long int smUint64a_t; +#endif + +#ifdef __APPLE__ +#define GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512 * 1024) +#else +#define GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (4 * 1024 * 1024) +#endif + +struct GraphicsCommand0 +{ + int bla; +}; + +struct GraphicsUpAxisCommand +{ + int m_enableUpAxisY; +}; + +struct GraphicsStatus0 +{ + int bla; +}; + +struct GraphicsVisualizerFlagCommand +{ + int m_visualizerFlag; + int m_enable; +}; + +struct GraphicsUploadDataCommand +{ + int m_numBytes; + int m_dataOffset; + int m_dataSlot; +}; + +struct GraphicRegisterTextureCommand +{ + int m_width; + int m_height; +}; + +struct GraphicsRegisterTextureStatus +{ + int m_textureId; +}; + +struct GraphicsRegisterGraphicsShapeCommand +{ + int m_numVertices; + int m_numIndices; + int m_primitiveType; + int m_textureId; +}; + +struct GraphicsRegisterGraphicsShapeStatus +{ + int m_shapeId; +}; + +struct GraphicsRegisterGraphicsInstanceCommand +{ + + int m_shapeIndex; + float m_position[4]; + float m_quaternion[4]; + float m_color[4]; + float m_scaling[4]; +}; + +struct GraphicsRegisterGraphicsInstanceStatus +{ + int m_graphicsInstanceId; +}; + +struct GraphicsSyncTransformsCommand +{ + int m_numPositions; +}; + +struct GraphicsRemoveInstanceCommand +{ + int m_graphicsUid; +}; + + +struct GraphicsSharedMemoryCommand +{ + int m_type; + smUint64a_t m_timeStamp; + int m_sequenceNumber; + + //m_updateFlags is a bit fields to tell which parameters need updating + //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; + int m_updateFlags; + + union { + struct GraphicsCommand0 m_graphicsCommand0; + struct GraphicsUpAxisCommand m_upAxisYCommand; + struct GraphicsVisualizerFlagCommand m_visualizerFlagCommand; + struct GraphicsUploadDataCommand m_uploadDataCommand; + struct GraphicRegisterTextureCommand m_registerTextureCommand; + struct GraphicsRegisterGraphicsShapeCommand m_registerGraphicsShapeCommand; + struct GraphicsRegisterGraphicsInstanceCommand m_registerGraphicsInstanceCommand; + struct GraphicsSyncTransformsCommand m_syncTransformsCommand; + struct GraphicsRemoveInstanceCommand m_removeGraphicsInstanceCommand; + }; +}; + +struct GraphicsSharedMemoryStatus +{ + int m_type; + + smUint64a_t m_timeStamp; + int m_sequenceNumber; + + //m_streamBytes is only for internal purposes + int m_numDataStreamBytes; + char* m_dataStream; + + //m_updateFlags is a bit fields to tell which parameters were updated, + //m_updateFlags is ignored for most status messages + int m_updateFlags; + + union { + + struct GraphicsStatus0 m_graphicsStatus0; + struct GraphicsRegisterTextureStatus m_registerTextureStatus; + struct GraphicsRegisterGraphicsShapeStatus m_registerGraphicsShapeStatus; + struct GraphicsRegisterGraphicsInstanceStatus m_registerGraphicsInstanceStatus; + }; +}; + +typedef struct GraphicsSharedMemoryStatus GraphicsSharedMemoryStatus_t; + +#endif //GRAPHICS_SHARED_MEMORY_COMMANDS_H diff --git a/examples/SharedMemory/GraphicsSharedMemoryPublic.h b/examples/SharedMemory/GraphicsSharedMemoryPublic.h new file mode 100644 index 000000000..99330cee2 --- /dev/null +++ b/examples/SharedMemory/GraphicsSharedMemoryPublic.h @@ -0,0 +1,47 @@ +#ifndef GRAPHICS_SHARED_MEMORY_PUBLIC_H +#define GRAPHICS_SHARED_MEMORY_PUBLIC_H + +#define GRAPHICS_SHARED_MEMORY_KEY 11347 +///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures +///my convention is year/month/day/rev +//Please don't replace an existing magic number: +//instead, only ADD a new one at the top, comment-out previous one + +#define GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER 201904030 +enum EnumGraphicsSharedMemoryClientCommand +{ + GFX_CMD_INVALID = 0, + 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, + //don't go beyond this command! + GFX_CMD_MAX_CLIENT_COMMANDS, +}; + +enum EnumGraphicsSharedMemoryServerStatus +{ + GFX_CMD_SHARED_MEMORY_NOT_INITIALIZED = 0, + //GFX_CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client + GFX_CMD_CLIENT_COMMAND_COMPLETED, + GFX_CMD_CLIENT_COMMAND_FAILED, + GFX_CMD_REGISTER_TEXTURE_COMPLETED, + GFX_CMD_REGISTER_TEXTURE_FAILED, + GFX_CMD_REGISTER_GRAPHICS_SHAPE_COMPLETED, + GFX_CMD_REGISTER_GRAPHICS_SHAPE_FAILED, + GFX_CMD_REGISTER_GRAPHICS_INSTANCE_COMPLETED, + GFX_CMD_REGISTER_GRAPHICS_INSTANCE_FAILED, + //don't go beyond 'CMD_MAX_SERVER_COMMANDS! + GFX_CMD_MAX_SERVER_COMMANDS +}; + + + + + +#endif //GRAPHICS_SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 87420a2ea..3c2f3aa73 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7717,6 +7717,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S } serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; + syncPhysicsToGraphics(); return hasStatus; } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index ce4c49267..95d54955c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -821,6 +821,46 @@ public: { m_childGuiHelper->syncPhysicsToGraphics(rbWorld); } + { + + b3AlignedObjectArray 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()); + } + } + } + + virtual void syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions) + { + m_childGuiHelper->syncPhysicsToGraphics2(positions, numPositions); } virtual void render(const btDiscreteDynamicsWorld* rbWorld) diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index cd7dc36ab..288196bdf 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -236,8 +236,10 @@ void PhysicsServerSharedMemory::reportNotifications() void PhysicsServerSharedMemory::processClientCommands() { + //handle client commands in any of the plugins m_data->m_commandProcessor->processClientCommands(); + //now handle the client commands from the shared memory for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++) { if (m_data->m_areConnected[block] && m_data->m_testBlocks[block]) diff --git a/examples/SharedMemory/SharedMemoryBlock.h b/examples/SharedMemory/SharedMemoryBlock.h index a9fd6d068..16c7a2fad 100644 --- a/examples/SharedMemory/SharedMemoryBlock.h +++ b/examples/SharedMemory/SharedMemoryBlock.h @@ -1,7 +1,7 @@ #ifndef SHARED_MEMORY_BLOCK_H #define SHARED_MEMORY_BLOCK_H -#define SHARED_MEMORY_MAX_COMMANDS 4 +#define SHARED_MEMORY_MAX_COMMANDS 1 #include "SharedMemoryCommands.h" diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 5df2d31d7..f64a084b1 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -10,6 +10,7 @@ #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../CommonInterfaces/CommonExampleInterface.h" #include "InProcessMemory.h" +#include "RemoteGUIHelper.h" #include "Bullet3Common/b3Logging.h" class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory @@ -281,6 +282,26 @@ B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingEx return (b3PhysicsClientHandle)cl; } +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect4(void* guiHelperPtr, int sharedMemoryKey) +{ + gSharedMemoryKey = sharedMemoryKey; + GUIHelperInterface* guiHelper = (GUIHelperInterface*)guiHelperPtr; + if (!guiHelper) + { + guiHelper = new RemoteGUIHelper(); + } + bool useInprocessMemory = false; + bool skipGraphicsUpdate = false; + InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate); + + cl->setSharedMemoryKey(sharedMemoryKey + 1); + cl->connect(); + //backward compatiblity + gSharedMemoryKey = SHARED_MEMORY_KEY; + return (b3PhysicsClientHandle)cl; +} + + //backward compatiblity B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr) { diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h index 6ed3d16c1..47d543f46 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h @@ -21,7 +21,9 @@ extern "C" B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr); //create a shared memory physics server, with a DummyGUIHelper (no graphics) and allow to set shared memory key 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); + ///ignore the following APIs, they are for internal use for example browser void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle); void b3InProcessDebugDrawInternal(b3PhysicsClientHandle clientHandle, int debugDrawMode); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 6c62c788e..13402ebf2 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -829,6 +829,7 @@ enum eCONNECT_METHOD eCONNECT_MUJOCO = 11, eCONNECT_GRPC = 12, eCONNECT_PHYSX=13, + eCONNECT_SHARED_MEMORY_GUI=14, }; enum eURDF_Flags diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 9d4bb28b9..cfa0d3aa1 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -34,6 +34,13 @@ myfiles = "PhysicsServer.cpp", "PhysicsServer.h", "PhysicsClientC_API.cpp", + "GraphicsClientExample.cpp", + "GraphicsClientExample.h", + "GraphicsServerExample.cpp", + "GraphicsServerExample.h", + "GraphicsSharedMemoryBlock.h", + "GraphicsSharedMemoryCommands.h", + "GraphicsSharedMemoryPublic.h", "SharedMemoryCommands.h", "SharedMemoryPublic.h", "PhysicsServer.cpp", diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py index d9699a369..235350765 100644 --- a/examples/pybullet/examples/inverse_kinematics.py +++ b/examples/pybullet/examples/inverse_kinematics.py @@ -5,7 +5,9 @@ from datetime import datetime clid = p.connect(p.SHARED_MEMORY) if (clid < 0): - p.connect(p.GUI) + #p.connect(p.GUI) + p.connect(p.SHARED_MEMORY_GUI) + p.loadURDF("plane.urdf", [0, 0, -0.3]) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1]) @@ -46,7 +48,9 @@ p.setRealTimeSimulation(useRealTimeSimulation) #use 0 for no-removal trailDuration = 15 -while 1: +i=0 +while i<5: + i+=1 p.getCameraImage(320, 200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, @@ -115,3 +119,4 @@ while 1: prevPose = pos prevPose1 = ls[4] hasPrevPose = 1 +p.disconnect() \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b884a155b..410241d73 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -463,6 +463,14 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect3(0, key); break; } + + case eCONNECT_SHARED_MEMORY_GUI: + { + sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect4(0, key); + break; + } + + case eCONNECT_DIRECT: { sm = b3ConnectPhysicsDirect(); @@ -10933,6 +10941,9 @@ initpybullet(void) PyModule_AddIntConstant(m, "GUI_SERVER", eCONNECT_GUI_SERVER); // user read 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 + + #ifdef BT_ENABLE_DART PyModule_AddIntConstant(m, "DART", eCONNECT_DART); // user read #endif From 708f66ac4200b4673985ee8d30d10800e513536c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 14 Jun 2019 08:06:21 -0700 Subject: [PATCH 4/7] add RemoteGUIHelper to communicate between graphics client/server --- examples/SharedMemory/RemoteGUIHelper.cpp | 550 ++++++++++++++++++++++ examples/SharedMemory/RemoteGUIHelper.h | 72 +++ 2 files changed, 622 insertions(+) create mode 100644 examples/SharedMemory/RemoteGUIHelper.cpp create mode 100644 examples/SharedMemory/RemoteGUIHelper.h diff --git a/examples/SharedMemory/RemoteGUIHelper.cpp b/examples/SharedMemory/RemoteGUIHelper.cpp new file mode 100644 index 000000000..80d1062a9 --- /dev/null +++ b/examples/SharedMemory/RemoteGUIHelper.cpp @@ -0,0 +1,550 @@ +#include "RemoteGUIHelper.h" + +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "Bullet3Common/b3Logging.h" +#include "GraphicsSharedMemoryCommands.h" +#include "PosixSharedMemory.h" +#include "Win32SharedMemory.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" + +struct RemoteGUIHelperInternalData +{ +// GUIHelperInterface* m_guiHelper; + bool m_waitingForServer; + GraphicsSharedMemoryBlock* m_testBlock1; + SharedMemoryInterface* m_sharedMemory; + GraphicsSharedMemoryStatus m_lastServerStatus; + int m_sharedMemoryKey; + bool m_isConnected; + + RemoteGUIHelperInternalData() + : m_waitingForServer(false), + m_testBlock1(0) + { +#ifdef _WIN32 + m_sharedMemory = new Win32SharedMemoryClient(); +#else + m_sharedMemory = new PosixSharedMemory(); +#endif + m_sharedMemoryKey = GRAPHICS_SHARED_MEMORY_KEY; + m_isConnected = false; + connect(); + + } + + virtual ~RemoteGUIHelperInternalData() + { + disconnect(); + delete m_sharedMemory; + } + + virtual bool isConnected() + { + return m_isConnected; + } + + bool canSubmitCommand() const + { + if (m_isConnected && !m_waitingForServer) + { + if (m_testBlock1->m_magicId == GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER) + { + return true; + } + else + { + return false; + } + } + return false; + } + + struct GraphicsSharedMemoryCommand* getAvailableSharedMemoryCommand() + { + static int sequence = 0; + if (m_testBlock1) + { + m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++; + return &m_testBlock1->m_clientCommands[0]; + } + return 0; + } + + bool submitClientCommand(const GraphicsSharedMemoryCommand& 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_waitingForServer); + if (!m_waitingForServer) + { + //printf("submit command of type %d\n", command.m_type); + + if (&m_testBlock1->m_clientCommands[0] != &command) + { + m_testBlock1->m_clientCommands[0] = command; + } + m_testBlock1->m_numClientCommands++; + m_waitingForServer = true; + return true; + } + return false; + } + + + const GraphicsSharedMemoryStatus* processServerStatus() + { + // SharedMemoryStatus* stat = 0; + + if (!m_testBlock1) + { + m_lastServerStatus.m_type = GFX_CMD_SHARED_MEMORY_NOT_INITIALIZED; + return &m_lastServerStatus; + } + + if (!m_waitingForServer) + { + return 0; + } + + if (m_testBlock1->m_magicId != GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER) + { + m_lastServerStatus.m_type = GFX_CMD_SHARED_MEMORY_NOT_INITIALIZED; + return &m_lastServerStatus; + } + + + if (m_testBlock1->m_numServerCommands > + m_testBlock1->m_numProcessedServerCommands) + { + B3_PROFILE("processServerCMD"); + b3Assert(m_testBlock1->m_numServerCommands == + m_testBlock1->m_numProcessedServerCommands + 1); + + const GraphicsSharedMemoryStatus& serverCmd = m_testBlock1->m_serverCommands[0]; + + m_lastServerStatus = serverCmd; + + // EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; + // consume the command + switch (serverCmd.m_type) + { + case GFX_CMD_CLIENT_COMMAND_COMPLETED: + { + B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED"); + + + break; + } + default: + { + } + } + + m_testBlock1->m_numProcessedServerCommands++; + // we don't have more than 1 command outstanding (in total, either server or client) + b3Assert(m_testBlock1->m_numProcessedServerCommands == + m_testBlock1->m_numServerCommands); + + if (m_testBlock1->m_numServerCommands == + m_testBlock1->m_numProcessedServerCommands) + { + m_waitingForServer = false; + } + else + { + m_waitingForServer = true; + } + + + return &m_lastServerStatus; + } + return 0; + } + + bool connect() + { + /// server always has to create and initialize shared memory + bool allowCreation = false; + m_testBlock1 = (GraphicsSharedMemoryBlock*)m_sharedMemory->allocateSharedMemory( + m_sharedMemoryKey, GRAPHICS_SHARED_MEMORY_SIZE, allowCreation); + + if (m_testBlock1) + { + if (m_testBlock1->m_magicId != GRAPHICS_SHARED_MEMORY_MAGIC_NUMBER) + { + b3Error("Error connecting to shared memory: please start server before client\n"); + m_sharedMemory->releaseSharedMemory(m_sharedMemoryKey, + GRAPHICS_SHARED_MEMORY_SIZE); + m_testBlock1 = 0; + return false; + } + else + { + m_isConnected = true; + } + } + else + { + b3Warning("Cannot connect to shared memory"); + return false; + } + return true; + } + + + void disconnect() + { + if (m_isConnected && m_sharedMemory) + { + m_sharedMemory->releaseSharedMemory(m_sharedMemoryKey, GRAPHICS_SHARED_MEMORY_SIZE); + } + m_isConnected = false; + } + +}; + + + + + + + +RemoteGUIHelper::RemoteGUIHelper() +{ + m_data = new RemoteGUIHelperInternalData; + if (m_data->canSubmitCommand()) + { + removeAllGraphicsInstances(); + } +} + +RemoteGUIHelper::~RemoteGUIHelper() +{ + delete m_data; +} + +void RemoteGUIHelper::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 RemoteGUIHelper::createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color) +{ + printf("createRigidBodyGraphicsObject\n"); +} + +void RemoteGUIHelper::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 RemoteGUIHelper::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) +{ + printf("createCollisionShapeGraphicsObject\n"); +} + +void RemoteGUIHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) +{ +} + +void RemoteGUIHelper::syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions) +{ + uploadData((unsigned char*) positions, numPositions * sizeof(GUISyncPosition), 0); + + GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); + if (cmd) + { + 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 RemoteGUIHelper::render(const btDiscreteDynamicsWorld* rbWorld) +{ +} + +void RemoteGUIHelper::createPhysicsDebugDrawer(btDiscreteDynamicsWorld* rbWorld) +{ +} + +int RemoteGUIHelper::uploadData(const unsigned char* data, int sizeInBytes, int slot) +{ + int chunkSize = GRAPHICS_SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; + int remainingBytes = sizeInBytes; + int offset = 0; + while (remainingBytes) + { + btAssert(remainingBytes >= 0); + int curBytes = btMin(remainingBytes, chunkSize); + GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); + if (cmd) + { + for (int i = 0; i < curBytes; i++) + { + m_data->m_testBlock1->m_bulletStreamData[i] = data[i]; + } + + cmd->m_updateFlags = 0; + cmd->m_type = GFX_CMD_UPLOAD_DATA; + cmd->m_uploadDataCommand.m_numBytes = curBytes; + 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) + { + } + offset += curBytes; + remainingBytes -= curBytes; + } + } + return 0; +} + +int RemoteGUIHelper::registerTexture(const unsigned char* texels, int width, int height) +{ + int textureId = -1; + + //first upload all data + int sizeInBytes = width*height * 3;//rgb + uploadData(texels, sizeInBytes, 0); + GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); + if (cmd) + { + 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 RemoteGUIHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId) +{ + int shapeId = -1; + + uploadData((unsigned char*)vertices, numvertices * 9*sizeof(float), 0); + uploadData((unsigned char*)indices, numIndices * sizeof(int), 1); + GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); + if (cmd) + { + 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 RemoteGUIHelper::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 RemoteGUIHelper::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 RemoteGUIHelper::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 RemoteGUIHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) +{ +} +Common2dCanvasInterface* RemoteGUIHelper::get2dCanvasInterface() +{ + return 0; +} + +CommonParameterInterface* RemoteGUIHelper::getParameterInterface() +{ + return 0; +} + +CommonRenderInterface* RemoteGUIHelper::getRenderInterface() +{ + return 0; +} + +CommonGraphicsApp* RemoteGUIHelper::getAppInterface() +{ + return 0; +} + +void RemoteGUIHelper::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 RemoteGUIHelper::resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ) +{ +} + +void RemoteGUIHelper::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 RemoteGUIHelper::setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]) +{ +} + +void RemoteGUIHelper::setProjectiveTexture(bool useProjectiveTexture) +{ +} + +void RemoteGUIHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) +{ +} + +void RemoteGUIHelper::drawText3D(const char* txt, float posX, float posZY, float posZ, float size) +{ +} + +void RemoteGUIHelper::drawText3D(const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag) +{ +} + +int RemoteGUIHelper::addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime, int trackingVisualShapeIndex, int replaceItemUid) +{ + return -1; +} +void RemoteGUIHelper::removeUserDebugItem(int debugItemUniqueId) +{ +} +void RemoteGUIHelper::removeAllUserDebugItems() +{ +} diff --git a/examples/SharedMemory/RemoteGUIHelper.h b/examples/SharedMemory/RemoteGUIHelper.h new file mode 100644 index 000000000..2acaaf007 --- /dev/null +++ b/examples/SharedMemory/RemoteGUIHelper.h @@ -0,0 +1,72 @@ +#ifndef REMOTE_HELPER_H +#define REMOTE_HELPER_H + +#include "../CommonInterfaces/CommonGUIHelperInterface.h" + +///a RemoteGUIHelper will connect to an existing graphics server through shared memory +struct RemoteGUIHelper : public GUIHelperInterface +{ + struct RemoteGUIHelperInternalData* m_data; + + RemoteGUIHelper(); + + virtual ~RemoteGUIHelper(); + + 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 void syncPhysicsToGraphics(const 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_H From ee3680765edd987314d44413f84d5f8283936850 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 14 Jun 2019 14:34:56 -0700 Subject: [PATCH 5/7] fixes for RemoteGUIHelper --- .../CommonGUIHelperInterface.h | 2 +- examples/ExampleBrowser/premake4.lua | 9 +++++ .../PhysicsServerCommandProcessor.cpp | 9 ++++- .../PhysicsServerCommandProcessor.h | 1 + .../SharedMemory/PhysicsServerExample.cpp | 40 +++---------------- examples/SharedMemory/RemoteGUIHelper.cpp | 39 ++++++++++++++++++ examples/SharedMemory/RemoteGUIHelper.h | 1 + examples/SharedMemory/premake4.lua | 2 + examples/pybullet/examples/quadruped.py | 4 +- examples/pybullet/premake4.lua | 9 +++++ setup.py | 2 + 11 files changed, 79 insertions(+), 39 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index fd56f879e..d29928e47 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -32,7 +32,7 @@ struct GUIHelperInterface virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) = 0; virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) = 0; - + virtual void syncPhysicsToGraphics2(const btDiscreteDynamicsWorld* rbWorld) {} virtual void syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions) {} virtual void render(const btDiscreteDynamicsWorld* rbWorld) = 0; diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index d23c782e5..b976136df 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -116,6 +116,15 @@ project "App_BulletExampleBrowser" "../SharedMemory/SharedMemoryCommandProcessor.cpp", "../SharedMemory/SharedMemoryCommandProcessor.h", "../SharedMemory/SharedMemoryInProcessPhysicsC_API.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/PhysicsClient.cpp", "../SharedMemory/PosixSharedMemory.cpp", "../SharedMemory/Win32SharedMemory.cpp", diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3c2f3aa73..26746dfb4 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7717,7 +7717,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S } serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; - syncPhysicsToGraphics(); + syncPhysicsToGraphics2(); return hasStatus; } @@ -11569,6 +11569,13 @@ void PhysicsServerCommandProcessor::syncPhysicsToGraphics() m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); } + +void PhysicsServerCommandProcessor::syncPhysicsToGraphics2() +{ + m_data->m_guiHelper->syncPhysicsToGraphics2(m_data->m_dynamicsWorld); +} + + void PhysicsServerCommandProcessor::renderScene(int renderFlags) { if (m_data->m_guiHelper) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 1b00a8af7..b770e246a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -138,6 +138,7 @@ public: virtual void physicsDebugDraw(int debugDrawFlags); virtual void setGuiHelper(struct GUIHelperInterface* guiHelper); virtual void syncPhysicsToGraphics(); + virtual void syncPhysicsToGraphics2(); //@todo(erwincoumans) Should we have shared memory commands for picking objects? ///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 95d54955c..eadded409 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -821,41 +821,11 @@ public: { m_childGuiHelper->syncPhysicsToGraphics(rbWorld); } - { - - b3AlignedObjectArray 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()); - } - } + } + + virtual void syncPhysicsToGraphics2(const btDiscreteDynamicsWorld* rbWorld) + { + m_childGuiHelper->syncPhysicsToGraphics2(rbWorld); } virtual void syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions) diff --git a/examples/SharedMemory/RemoteGUIHelper.cpp b/examples/SharedMemory/RemoteGUIHelper.cpp index 80d1062a9..079bee2f6 100644 --- a/examples/SharedMemory/RemoteGUIHelper.cpp +++ b/examples/SharedMemory/RemoteGUIHelper.cpp @@ -12,6 +12,7 @@ #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "Bullet3Common/b3AlignedObjectArray.h" +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" struct RemoteGUIHelperInternalData { @@ -282,6 +283,44 @@ void RemoteGUIHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWor { } +void RemoteGUIHelper::syncPhysicsToGraphics2(const btDiscreteDynamicsWorld* rbWorld) +{ + b3AlignedObjectArray 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 RemoteGUIHelper::syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions) { uploadData((unsigned char*) positions, numPositions * sizeof(GUISyncPosition), 0); diff --git a/examples/SharedMemory/RemoteGUIHelper.h b/examples/SharedMemory/RemoteGUIHelper.h index 2acaaf007..5e87f1d5c 100644 --- a/examples/SharedMemory/RemoteGUIHelper.h +++ b/examples/SharedMemory/RemoteGUIHelper.h @@ -21,6 +21,7 @@ struct RemoteGUIHelper : public GUIHelperInterface virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape); 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); diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index cfa0d3aa1..1be7f957f 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -41,6 +41,8 @@ myfiles = "GraphicsSharedMemoryBlock.h", "GraphicsSharedMemoryCommands.h", "GraphicsSharedMemoryPublic.h", + "RemoteGUIHelper.cpp", + "RemoteGUIHelper.h", "SharedMemoryCommands.h", "SharedMemoryPublic.h", "PhysicsServer.cpp", diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index a9352fd08..6d72abb86 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/examples/pybullet/examples/quadruped.py @@ -104,7 +104,7 @@ def drawInertiaBox(parentUid, parentLinkIndex, color): toeConstraint = True useMaximalCoordinates = False -useRealTime = 1 +useRealTime = 0 #the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance fixedTimeStep = 1. / 100 @@ -123,7 +123,7 @@ kp = 1 kd = .5 maxKneeForce = 1000 -physId = p.connect(p.SHARED_MEMORY) +physId = p.connect(p.SHARED_MEMORY_GUI) if (physId < 0): p.connect(p.GUI) #p.resetSimulation() diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 1832dd205..3939dfc85 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -124,6 +124,15 @@ if not _OPTIONS["no-enet"] then "../../examples/SharedMemory/PhysicsServer.h", "../../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", "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", "../../examples/SharedMemory/PhysicsServerSharedMemory.h", diff --git a/setup.py b/setup.py index 190b7a1e1..0d194520f 100644 --- a/setup.py +++ b/setup.py @@ -117,6 +117,8 @@ sources = ["examples/pybullet/pybullet.c"]\ +["examples/SharedMemory/InProcessMemory.cpp"]\ +["examples/SharedMemory/PhysicsClient.cpp"]\ +["examples/SharedMemory/PhysicsServer.cpp"]\ ++["examples/SharedMemory/GraphicsClientExample.cpp"]\ ++["examples/SharedMemory/RemoteGUIHelper.cpp"]\ +["examples/SharedMemory/PhysicsServerExample.cpp"]\ +["examples/SharedMemory/PhysicsServerExampleBullet2.cpp"]\ +["examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp"]\ From e286fbd9f29b018b30963411c137ab8d15bcddb0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 15 Jun 2019 10:30:48 -0700 Subject: [PATCH 6/7] fixes for RemoteGUIHelper --- examples/ExampleBrowser/CMakeLists.txt | 8 ++++++++ examples/RobotSimulator/CMakeLists.txt | 8 ++++++++ examples/SharedMemory/RemoteGUIHelper.cpp | 2 +- examples/TwoJoint/CMakeLists.txt | 8 ++++++++ examples/pybullet/CMakeLists.txt | 8 ++++++++ examples/pybullet/examples/inverse_kinematics.py | 16 ++++++++-------- 6 files changed, 41 insertions(+), 9 deletions(-) diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index d17ed4210..a04e9f9b4 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -171,6 +171,14 @@ SET(BulletExampleBrowser_SRCS ../SharedMemory/PhysicsClientSharedMemory_C_API.cpp ../SharedMemory/PhysicsClient.cpp ../SharedMemory/PhysicsClientC_API.cpp + ../SharedMemory/GraphicsServerExample.cpp + ../SharedMemory/GraphicsClientExample.cpp + ../SharedMemory/RemoteGUIHelper.cpp + ../SharedMemory/GraphicsServerExample.h + ../SharedMemory/GraphicsClientExample.h + ../SharedMemory/RemoteGUIHelper.h + ../SharedMemory/GraphicsSharedMemoryCommands.h + ../SharedMemory/GraphicsSharedMemoryPublic.h ../SharedMemory/PhysicsServerExample.cpp ../SharedMemory/PhysicsServerExampleBullet2.cpp ../SharedMemory/PhysicsClientExample.cpp diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt index 6a0553a76..8f0194caf 100644 --- a/examples/RobotSimulator/CMakeLists.txt +++ b/examples/RobotSimulator/CMakeLists.txt @@ -15,6 +15,14 @@ RobotSimulatorMain.cpp MinitaurSetup.cpp MinitaurSetup.h ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp + ../SharedMemory/GraphicsServerExample.cpp + ../SharedMemory/GraphicsClientExample.cpp + ../SharedMemory/RemoteGUIHelper.cpp + ../SharedMemory/GraphicsServerExample.h + ../SharedMemory/GraphicsClientExample.h + ../SharedMemory/RemoteGUIHelper.h + ../SharedMemory/GraphicsSharedMemoryCommands.h + ../SharedMemory/GraphicsSharedMemoryPublic.h ../../examples/SharedMemory/PhysicsServerExample.cpp ../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp diff --git a/examples/SharedMemory/RemoteGUIHelper.cpp b/examples/SharedMemory/RemoteGUIHelper.cpp index 079bee2f6..3c71a772f 100644 --- a/examples/SharedMemory/RemoteGUIHelper.cpp +++ b/examples/SharedMemory/RemoteGUIHelper.cpp @@ -363,7 +363,7 @@ int RemoteGUIHelper::uploadData(const unsigned char* data, int sizeInBytes, int { for (int i = 0; i < curBytes; i++) { - m_data->m_testBlock1->m_bulletStreamData[i] = data[i]; + m_data->m_testBlock1->m_bulletStreamData[i] = data[i+offset]; } cmd->m_updateFlags = 0; diff --git a/examples/TwoJoint/CMakeLists.txt b/examples/TwoJoint/CMakeLists.txt index a596ab404..6069d58b5 100644 --- a/examples/TwoJoint/CMakeLists.txt +++ b/examples/TwoJoint/CMakeLists.txt @@ -32,6 +32,14 @@ SET(RobotSimulator_SRCS ../../examples/SharedMemory/PhysicsClient.h ../../examples/SharedMemory/PhysicsServer.cpp ../../examples/SharedMemory/PhysicsServer.h + ../SharedMemory/GraphicsServerExample.cpp + ../SharedMemory/GraphicsClientExample.cpp + ../SharedMemory/RemoteGUIHelper.cpp + ../SharedMemory/GraphicsServerExample.h + ../SharedMemory/GraphicsClientExample.h + ../SharedMemory/RemoteGUIHelper.h + ../SharedMemory/GraphicsSharedMemoryCommands.h + ../SharedMemory/GraphicsSharedMemoryPublic.h ../../examples/SharedMemory/PhysicsServerExample.cpp ../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 1b791ec7e..229b67245 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -55,6 +55,14 @@ SET(pybullet_SRCS ../../examples/SharedMemory/PhysicsClient.h ../../examples/SharedMemory/PhysicsServer.cpp ../../examples/SharedMemory/PhysicsServer.h + ../SharedMemory/GraphicsServerExample.cpp + ../SharedMemory/GraphicsClientExample.cpp + ../SharedMemory/RemoteGUIHelper.cpp + ../SharedMemory/GraphicsServerExample.h + ../SharedMemory/GraphicsClientExample.h + ../SharedMemory/RemoteGUIHelper.h + ../SharedMemory/GraphicsSharedMemoryCommands.h + ../SharedMemory/GraphicsSharedMemoryPublic.h ../../examples/SharedMemory/PhysicsServerExample.cpp ../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py index 235350765..803280b0d 100644 --- a/examples/pybullet/examples/inverse_kinematics.py +++ b/examples/pybullet/examples/inverse_kinematics.py @@ -40,8 +40,8 @@ useNullSpace = 1 useOrientation = 1 #If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control. #This can be used to test the IK result accuracy. -useSimulation = 0 -useRealTimeSimulation = 1 +useSimulation = 1 +useRealTimeSimulation = 0 ikSolver = 0 p.setRealTimeSimulation(useRealTimeSimulation) #trailDuration is duration (in seconds) after debug lines will be removed automatically @@ -49,17 +49,17 @@ p.setRealTimeSimulation(useRealTimeSimulation) trailDuration = 15 i=0 -while i<5: +while 1: i+=1 - p.getCameraImage(320, - 200, - flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, - renderer=p.ER_BULLET_HARDWARE_OPENGL) + #p.getCameraImage(320, + # 200, + # flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, + # renderer=p.ER_BULLET_HARDWARE_OPENGL) if (useRealTimeSimulation): dt = datetime.now() t = (dt.second / 60.) * 2. * math.pi else: - t = t + 0.001 + t = t + 0.01 if (useSimulation and useRealTimeSimulation == 0): p.stepSimulation() From 9d21deb712345ad161d178b08022959a5323d4ae Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 15 Jun 2019 12:58:02 -0700 Subject: [PATCH 7/7] use GUI mode in IK example --- examples/pybullet/examples/inverse_kinematics.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py index 803280b0d..11ec4ed4f 100644 --- a/examples/pybullet/examples/inverse_kinematics.py +++ b/examples/pybullet/examples/inverse_kinematics.py @@ -5,8 +5,8 @@ from datetime import datetime clid = p.connect(p.SHARED_MEMORY) if (clid < 0): - #p.connect(p.GUI) - p.connect(p.SHARED_MEMORY_GUI) + p.connect(p.GUI) + #p.connect(p.SHARED_MEMORY_GUI) p.loadURDF("plane.urdf", [0, 0, -0.3]) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0]) @@ -119,4 +119,4 @@ while 1: prevPose = pos prevPose1 = ls[4] hasPrevPose = 1 -p.disconnect() \ No newline at end of file +p.disconnect()