diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 9c0de410d..efc8ab295 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -38,7 +38,8 @@ struct GUIHelperInterface virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) =0; virtual void removeAllGraphicsInstances()=0; virtual void removeGraphicsInstance(int graphicsUid) {} - + virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} + virtual Common2dCanvasInterface* get2dCanvasInterface()=0; virtual CommonParameterInterface* getParameterInterface()=0; @@ -125,6 +126,7 @@ struct DummyGUIHelper : public GUIHelperInterface virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) {return -1;} virtual void removeAllGraphicsInstances(){} virtual void removeGraphicsInstance(int graphicsUid){} + virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} virtual Common2dCanvasInterface* get2dCanvasInterface() { diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index bbc20c6f9..4af517782 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -55,10 +55,10 @@ struct CommonRenderInterface virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)=0; virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)=0; - virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex)=0; - virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex)=0; - virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex)=0; - virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex)=0; + virtual void writeSingleInstanceColorToCPU(const float* color, int srcIndex)=0; + virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex)=0; + virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex)=0; + virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex)=0; virtual int getTotalNumInstances() const = 0; diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 8b8d7cab6..23c283fcb 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -193,6 +193,8 @@ SET(BulletExampleBrowser_SRCS ../SharedMemory/PhysicsServerCommandProcessor.h ../SharedMemory/SharedMemoryCommands.h ../SharedMemory/SharedMemoryPublic.h + ../RobotSimulator/b3RobotSimulatorClientAPI.cpp + ../RobotSimulator/b3RobotSimulatorClientAPI.h ../BasicDemo/BasicExample.cpp ../BasicDemo/BasicExample.h ../InverseDynamics/InverseDynamicsExample.cpp @@ -254,8 +256,6 @@ SET(BulletExampleBrowser_SRCS ../RoboticsLearning/GripperGraspExample.cpp ../RoboticsLearning/GripperGraspExample.h - ../RoboticsLearning/b3RobotSimAPI.cpp - ../RoboticsLearning/b3RobotSimAPI.h ../RoboticsLearning/R2D2GraspExample.cpp ../RoboticsLearning/R2D2GraspExample.h ../RoboticsLearning/KukaGraspExample.cpp diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 39cb5ebf4..533532a54 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -314,6 +314,14 @@ void OpenGLGuiHelper::removeGraphicsInstance(int graphicsUid) }; } +void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) +{ + if (instanceUid>=0) + { + m_data->m_glApp->m_renderer->writeSingleInstanceColorToCPU(rgbaColor,instanceUid); + }; +} + int OpenGLGuiHelper::createCheckeredTexture(int red,int green, int blue) { int texWidth=1024; diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index db4a12796..8a0dd67dc 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -26,7 +26,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface 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 void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape); virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld); diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index dc1a0b4d1..e029b8edf 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -118,6 +118,8 @@ project "App_BulletExampleBrowser" "../MultiThreading/b3ThreadSupportInterface.cpp", "../InverseDynamics/InverseDynamicsExample.cpp", "../InverseDynamics/InverseDynamicsExample.h", + "../RobotSimulator/b3RobotSimulatorClientAPI.cpp", + "../RobotSimulator/b3RobotSimulatorClientAPI.h", "../BasicDemo/BasicExample.*", "../Tutorial/*", "../ExtendedTutorials/*", diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 32cc0e03e..39a8c8a5b 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -375,7 +375,7 @@ void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int bodyUniqueId, orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2]; orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3]; } -void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int bodyUniqueId) +void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, int bodyUniqueId) { b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId); b3Assert(pg); @@ -387,7 +387,7 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int body m_data->m_instance_colors_ptr[srcIndex*4+3]=float(color[3]); } -void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int bodyUniqueId) +void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int bodyUniqueId) { b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId); b3Assert(pg); @@ -399,7 +399,7 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int bodyU m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3]; } -void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int bodyUniqueId) +void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int bodyUniqueId) { b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId); b3Assert(pg); @@ -410,7 +410,7 @@ void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int bodyU m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2]; } -void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int bodyUniqueId) +void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const double* scale, int bodyUniqueId) { b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId); b3Assert(pg); diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index b49cf2107..be063e0ab 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/examples/OpenGLWindow/GLInstancingRenderer.h @@ -98,11 +98,11 @@ public: virtual void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex); - virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex); - virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex); + virtual void writeSingleInstanceColorToCPU(const float* color, int srcIndex); + virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex); - virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex); - virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex); + virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex); + virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex); virtual struct GLInstanceRendererInternalData* getInternalData(); diff --git a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl index 1e469134c..ad12e3b0c 100644 --- a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl +++ b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl @@ -23,10 +23,12 @@ out vec4 color; void main(void) { - vec4 texel = fragment.color*texture(Diffuse,vert.texcoord); + vec4 texel = fragment.color*texture(Diffuse,vert.texcoord); vec3 ct,cf; float intensity,at,af; - + if (fragment.color.w==0) + discard; + intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 ); af = 1.0; @@ -45,6 +47,5 @@ void main(void) intensity = 0.7*intensity + 0.3*intensity*visibility; cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient; - color = vec4(ct * cf, fragment.color.w); } diff --git a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h index de316f9d7..c2a6a306f 100644 --- a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h +++ b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h @@ -17,10 +17,11 @@ static const char* useShadowMapInstancingFragmentShader= \ "out vec4 color;\n" "void main(void)\n" "{\n" -" vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);\n" +" vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);\n" " vec3 ct,cf;\n" " float intensity,at,af;\n" -" \n" +" if (fragment.color.w==0)\n" +" discard;\n" " intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );\n" " \n" " af = 1.0;\n" @@ -37,7 +38,6 @@ static const char* useShadowMapInstancingFragmentShader= \ " intensity = 0.7*intensity + 0.3*intensity*visibility;\n" " \n" " cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;\n" -" \n" " color = vec4(ct * cf, fragment.color.w);\n" "}\n" ; diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index 564212f9d..25a3c2cfa 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp @@ -26,7 +26,7 @@ B3_ATTRIBUTE_ALIGNED16(struct) SimpleGL2Instance int m_shapeIndex; b3Vector3 m_position; b3Quaternion orn; - b3Vector3 m_rgbColor; + b3Vector4 m_rgbColor; b3Vector3 m_scaling; void clear() { @@ -139,18 +139,18 @@ void SimpleOpenGL2Renderer::removeGraphicsInstance(int instanceUid) m_data->m_graphicsInstancesPool.freeHandle(instanceUid); } -void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(float* color, int srcIndex) +void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(const float* color, int srcIndex) { } -void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(double* color, int srcIndex) +void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(const double* color, int srcIndex) { } -void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex) +void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(const float* scale, int srcIndex) { } -void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex) +void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(const double* scale, int srcIndex) { } @@ -182,6 +182,10 @@ void SimpleOpenGL2Renderer::drawOpenGL(int instanceIndex) const SimpleGL2Instance& inst = *instPtr; const SimpleGL2Shape* shape = m_data->m_shapes[inst.m_shapeIndex]; + if (inst.m_rgbColor[3]==0) + { + return; + } glPushMatrix(); b3Transform tr; @@ -475,6 +479,8 @@ int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const double instance.m_rgbColor[0] = color[0]; instance.m_rgbColor[1] = color[1]; instance.m_rgbColor[2] = color[2]; + instance.m_rgbColor[3] = color[3]; + instance.m_scaling[0] = scaling[0]; instance.m_scaling[1] = scaling[1]; instance.m_scaling[2] = scaling[2]; @@ -496,6 +502,8 @@ int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const float* instance.m_rgbColor[0] = color[0]; instance.m_rgbColor[1] = color[1]; instance.m_rgbColor[2] = color[2]; + instance.m_rgbColor[3] = color[3]; + instance.m_scaling[0] = scaling[0]; instance.m_scaling[1] = scaling[1]; instance.m_scaling[2] = scaling[2]; diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h index 3a94a3783..7eb9ab809 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h @@ -34,10 +34,10 @@ public: virtual void removeAllInstances(); virtual void removeGraphicsInstance(int instanceUid); - virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex); - virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex); - virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex); - virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex); + virtual void writeSingleInstanceColorToCPU(const float* color, int srcIndex); + virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex); + virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex); + virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex); virtual void getCameraViewMatrix(float viewMat[16]) const; virtual void getCameraProjectionMatrix(float projMat[16]) const; diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 5e4eefa74..fd744cea9 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -2,29 +2,32 @@ //#include "SharedMemoryCommands.h" -#include "SharedMemory/PhysicsClientC_API.h" +#include "../SharedMemory/PhysicsClientC_API.h" #ifdef BT_ENABLE_ENET -#include "SharedMemory/PhysicsClientUDP_C_API.h" +#include "../SharedMemory/PhysicsClientUDP_C_API.h" #endif //PHYSICS_UDP #ifdef BT_ENABLE_CLSOCKET -#include "SharedMemory/PhysicsClientTCP_C_API.h" +#include "../SharedMemory/PhysicsClientTCP_C_API.h" #endif //PHYSICS_TCP -#include "SharedMemory/PhysicsDirectC_API.h" +#include "../SharedMemory/PhysicsDirectC_API.h" -#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h" +#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" -#include "SharedMemory/SharedMemoryPublic.h" + +#include "../SharedMemory/SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" struct b3RobotSimulatorClientAPI_InternalData { b3PhysicsClientHandle m_physicsClientHandle; + struct GUIHelperInterface* m_guiHelper; b3RobotSimulatorClientAPI_InternalData() - : m_physicsClientHandle(0) + : m_physicsClientHandle(0), + m_guiHelper(0) { } }; @@ -39,6 +42,66 @@ b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() delete m_data; } +void b3RobotSimulatorClientAPI::setGuiHelper(struct GUIHelperInterface* guiHelper) +{ + m_data->m_guiHelper = guiHelper; +} + +void b3RobotSimulatorClientAPI::renderScene() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + if (m_data->m_guiHelper) + { + b3InProcessRenderSceneInternal(m_data->m_physicsClientHandle); + } +} + +void b3RobotSimulatorClientAPI::debugDraw(int debugDrawMode) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + if (m_data->m_guiHelper) + { + b3InProcessDebugDrawInternal(m_data->m_physicsClientHandle,debugDrawMode); + } +} + +bool b3RobotSimulatorClientAPI::mouseMoveCallback(float x,float y) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + if (m_data->m_guiHelper) + { + return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y); + } + return false; +} +bool b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float x, float y) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + if (m_data->m_guiHelper) + { + return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y); + } + return false; +} + + + bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey) { if (m_data->m_physicsClientHandle) @@ -55,6 +118,12 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i switch (mode) { + case eCONNECT_EXISTING_EXAMPLE_BROWSER: + { + sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper); + break; + } + case eCONNECT_GUI: { int argc = 0; @@ -644,8 +713,72 @@ bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, return false; } +bool b3RobotSimulatorClientAPI::getJointStates(int bodyUniqueId, b3JointStates2& state) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (statusHandle) + { + // double rootInertialFrame[7]; + const double* rootLocalInertialFrame; + const double* actualStateQ; + const double* actualStateQdot; + const double* jointReactionForces; + + int stat = b3GetStatusActualState(statusHandle, + &state.m_bodyUniqueId, + &state.m_numDegreeOfFreedomQ, + &state.m_numDegreeOfFreedomU, + &rootLocalInertialFrame, + &actualStateQ, + &actualStateQdot, + &jointReactionForces); + if (stat) + { + state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); + state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); + + for (int i=0;im_physicsClientHandle, commandHandle); +} + +void b3RobotSimulatorClientAPI::loadBunny(double scale, double mass, double collisionMargin) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClientHandle); + b3LoadBunnySetScale(command, scale); + b3LoadBunnySetMass(command, mass); + b3LoadBunnySetCollisionMargin(command, collisionMargin); + b3SubmitClientCommand(m_data->m_physicsClientHandle, command); } \ No newline at end of file diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index 7dd9fdeb4..fadc8e79d 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -124,6 +124,17 @@ struct b3RobotSimulatorInverseKinematicsResults b3AlignedObjectArray m_calculatedJointPositions; }; +struct b3JointStates2 +{ + int m_bodyUniqueId; + int m_numDegreeOfFreedomQ; + int m_numDegreeOfFreedomU; + b3Transform m_rootLocalInertialFrame; + b3AlignedObjectArray m_actualStateQ; + b3AlignedObjectArray m_actualStateQdot; + b3AlignedObjectArray m_jointReactionForces; +}; + ///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet ///as documented in the pybullet Quickstart Guide ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA @@ -177,6 +188,8 @@ public: bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); + bool getJointStates(int bodyUniqueId, b3JointStates2& state); + bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); @@ -213,6 +226,21 @@ public: void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1); + + //////////////// INTERNAL + + void loadBunny(double scale, double mass, double collisionMargin); + + //setGuiHelper is only used when embedded in existing example browser + void setGuiHelper(struct GUIHelperInterface* guiHelper); + //renderScene is only used when embedded in existing example browser + virtual void renderScene(); + //debugDraw is only used when embedded in existing example browser + virtual void debugDraw(int debugDrawMode); + virtual bool mouseMoveCallback(float x,float y); + virtual bool mouseButtonCallback(int button, int state, float x, float y); + + ////////////////INTERNAL }; #endif //B3_ROBOT_SIMULATOR_CLIENT_API_H diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua index df4cf88a5..f2cba47c7 100644 --- a/examples/RobotSimulator/premake4.lua +++ b/examples/RobotSimulator/premake4.lua @@ -199,6 +199,7 @@ if not _OPTIONS["no-enet"] then end +if _OPTIONS["serial"] then project ("App_VRGloveHandSimulator") @@ -248,12 +249,11 @@ project ("App_VRGloveHandSimulator") } defines {"B3_ENABLE_TINY_AUDIO"} - if _OPTIONS["serial"] then - defines{"B3_ENABLE_SERIAL"} - includedirs {"../../examples/ThirdPartyLibs/serial/include"} - links {"serial"} - end + defines{"B3_ENABLE_SERIAL"} + includedirs {"../../examples/ThirdPartyLibs/serial/include"} + links {"serial"} + if os.is("Windows") then links {"winmm","Wsock32","dsound"} defines {"WIN32","__WINDOWS_MM__","__WINDOWS_DS__"} @@ -280,3 +280,4 @@ project ("App_VRGloveHandSimulator") if os.is("Linux") then initX11() end +end \ No newline at end of file diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 60b4d3e4e..a01993c93 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -13,7 +13,7 @@ #include "../SharedMemory/SharedMemoryPublic.h" #include -#include "b3RobotSimAPI.h" +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" #include "../Utils/b3Clock.h" static btScalar sGripperVerticalVelocity = 0.f; @@ -23,7 +23,7 @@ class GripperGraspExample : public CommonExampleInterface { CommonGraphicsApp* m_app; GUIHelperInterface* m_guiHelper; - b3RobotSimAPI m_robotSim; + b3RobotSimulatorClientAPI m_robotSim; int m_options; int m_gripperIndex; @@ -55,8 +55,12 @@ public: } virtual void initPhysics() { - bool connected = m_robotSim.connect(m_guiHelper); - b3Printf("robotSim connected = %d",connected); + + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + + b3Printf("robotSim connected = %d",connected); if ((m_options & eGRIPPER_GRASP)!=0) { @@ -76,23 +80,18 @@ public: m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.urdf"; + + b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0, 0, .107); args.m_startOrientation.setEulerZYX(0, 0, 0); args.m_useMultiBody = true; - m_robotSim.loadFile(args, results); + m_robotSim.loadURDF("cube_small.urdf", args); } { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; - args.m_fileType = B3_SDF_FILE; - args.m_useMultiBody = true; - b3RobotSimLoadFileResults results; - - if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + b3RobotSimulatorLoadFileResults results; + m_robotSim.loadSDF("gripper/wsg50_with_r2d2_gripper.sdf",results); + if (results.m_uniqueObjectIds.size()==1) { m_gripperIndex = results.m_uniqueObjectIds[0]; @@ -111,7 +110,7 @@ public: double fingerTargetPositions[2]={-0.04,0.04}; for (int i=0;i<2;i++) { - b3JointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); controlArgs.m_targetPosition = fingerTargetPositions[i]; controlArgs.m_kp = 5.0; controlArgs.m_kd = 3.0; @@ -124,7 +123,7 @@ public: double maxTorqueValues[3]={40.0,50.0,50.0}; for (int i=0;i<3;i++) { - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_kd = 1.; @@ -135,15 +134,7 @@ public: if (1) { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "plane.urdf"; - args.m_startPosition.setValue(0,0,0); - args.m_startOrientation.setEulerZYX(0,0,0); - args.m_forceOverrideFixedBase = true; - args.m_useMultiBody = true; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); - + m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setNumSimulationSubSteps(4); @@ -152,24 +143,17 @@ public: if ((m_options & eTWO_POINT_GRASP)!=0) { { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.urdf"; + b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0, 0, .107); - args.m_startOrientation.setEulerZYX(0, 0, 0); - args.m_useMultiBody = true; - m_robotSim.loadFile(args, results); + m_robotSim.loadURDF("cube_small.urdf", args); } { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; - args.m_fileName = "cube_gripper_left.urdf"; + b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0.068, 0.02, 0.11); - args.m_useMultiBody = true; - m_robotSim.loadFile(args, results); + m_robotSim.loadURDF("cube_gripper_left.urdf", args); - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = -0.1; controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_kd = 1.; @@ -177,14 +161,11 @@ public: } { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; - args.m_fileName = "cube_gripper_right.urdf"; + b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(-0.068, 0.02, 0.11); - args.m_useMultiBody = true; - m_robotSim.loadFile(args, results); + m_robotSim.loadURDF("cube_gripper_right.urdf", args); - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = 0.1; controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_kd = 1.; @@ -193,14 +174,7 @@ public: if (1) { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "plane.urdf"; - args.m_startPosition.setValue(0,0,0); - args.m_startOrientation.setEulerZYX(0,0,0); - args.m_forceOverrideFixedBase = true; - args.m_useMultiBody = true; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); + m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); @@ -225,22 +199,17 @@ public: m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; - args.m_fileName = "dinnerware/pan_tefal.urdf"; + b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0, -0.2, .47); args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0); - args.m_useMultiBody = true; - m_robotSim.loadFile(args, results); + m_robotSim.loadURDF("dinnerware/pan_tefal.urdf", args); } { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; - args.m_fileType = B3_SDF_FILE; - args.m_useMultiBody = true; - b3RobotSimLoadFileResults results; - - if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + b3RobotSimulatorLoadFileResults args; + b3RobotSimulatorLoadFileResults results; + m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results); + + if (results.m_uniqueObjectIds.size()==1) { m_gripperIndex = results.m_uniqueObjectIds[0]; @@ -256,7 +225,7 @@ public: for (int i=0;i<8;i++) { - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs); } @@ -266,15 +235,7 @@ public: if (1) { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "plane.urdf"; - args.m_startPosition.setValue(0,0,0); - args.m_startOrientation.setEulerZYX(0,0,0); - args.m_forceOverrideFixedBase = true; - args.m_useMultiBody = true; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); - + m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); @@ -316,8 +277,8 @@ public: revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointType = ePoint2PointType; - m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1); - m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2); + m_robotSim.createConstraint(1, 2, 1, 4, &revoluteJoint1); + m_robotSim.createConstraint(1, 3, 1, 6, &revoluteJoint2); } if ((m_options & eGRASP_SOFT_BODY)!=0) @@ -337,13 +298,10 @@ public: m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; - args.m_fileType = B3_SDF_FILE; - args.m_useMultiBody = true; - b3RobotSimLoadFileResults results; - - if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + b3RobotSimulatorLoadFileResults results; + m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results); + + if (results.m_uniqueObjectIds.size()==1) { m_gripperIndex = results.m_uniqueObjectIds[0]; @@ -359,7 +317,7 @@ public: for (int i=0;i<8;i++) { - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs); } @@ -367,14 +325,10 @@ public: } } { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "plane.urdf"; + b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,0,-0.2); args.m_startOrientation.setEulerZYX(0,0,0); - args.m_forceOverrideFixedBase = true; - args.m_useMultiBody = true; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); + m_robotSim.loadURDF("plane.urdf", args); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); @@ -418,23 +372,20 @@ public: revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointType = ePoint2PointType; - m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint1); - m_robotSim.createJoint(0, 3, 0, 6, &revoluteJoint2); + m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1); + m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2); } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0) { { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "kuka_iiwa/model_free_base.urdf"; + b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,1.0,2.0); args.m_startOrientation.setEulerZYX(0,0,1.57); args.m_forceOverrideFixedBase = false; args.m_useMultiBody = true; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); + int kukaId = m_robotSim.loadURDF("kuka_iiwa/model_free_base.urdf", args); - int kukaId = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(kukaId); b3Printf("numJoints = %d",numJoints); @@ -443,20 +394,18 @@ public: b3JointInfo jointInfo; m_robotSim.getJointInfo(kukaId,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; m_robotSim.setJointMotorControl(kukaId,i,controlArgs); } } { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "plane.urdf"; + b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0); args.m_forceOverrideFixedBase = true; args.m_useMultiBody = false; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); + m_robotSim.loadURDF("plane.urdf", args); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.loadBunny(0.5,10.0,0.1); @@ -479,7 +428,7 @@ public: double maxTorqueValues[3]={40.0,50.0,50.0}; for (int i=0;i<3;i++) { - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_kd = 1.; @@ -496,7 +445,7 @@ public: double maxTorqueValues[2]={800.0,800.0}; for (int i=0;i<2;i++) { - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_kd = 1.; @@ -512,7 +461,7 @@ public: double maxTorqueValues[2]={50.0,10.0}; for (int i=0;i<2;i++) { - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_kd = 1.; @@ -531,17 +480,13 @@ public: } - virtual void physicsDebugDraw() - { - - } virtual bool mouseMoveCallback(float x,float y) { - return false; + return m_robotSim.mouseMoveCallback(x,y); } virtual bool mouseButtonCallback(int button, int state, float x, float y) { - return false; + return m_robotSim.mouseButtonCallback(button,state,x,y); } virtual bool keyboardCallback(int key, int state) { diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index b116c0faa..abb3005d1 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -11,8 +11,8 @@ #include "../SharedMemory/PhysicsServerSharedMemory.h" #include "../SharedMemory/PhysicsClientC_API.h" #include +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" -#include "b3RobotSimAPI.h" #include "../Utils/b3Clock.h" ///quick demo showing the right-handed coordinate system and positive rotations around each axis @@ -20,7 +20,8 @@ class KukaGraspExample : public CommonExampleInterface { CommonGraphicsApp* m_app; GUIHelperInterface* m_guiHelper; - b3RobotSimAPI m_robotSim; + b3RobotSimulatorClientAPI m_robotSim; + int m_kukaIndex; IKTrajectoryHelper m_ikHelper; @@ -57,10 +58,7 @@ public: } - virtual void physicsDebugDraw(int debugDrawMode) - { - - } + virtual void initPhysics() { @@ -78,19 +76,18 @@ public: - - bool connected = m_robotSim.connect(m_guiHelper); + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + +// 0;//m_robotSim.connect(m_guiHelper); b3Printf("robotSim connected = %d",connected); { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "kuka_iiwa/model.urdf"; - args.m_startPosition.setValue(0,0,0); - b3RobotSimLoadFileResults results; - if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + m_kukaIndex = m_robotSim.loadURDF("kuka_iiwa/model.urdf"); + if (m_kukaIndex >=0) { - m_kukaIndex = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(m_kukaIndex); b3Printf("numJoints = %d",numJoints); @@ -112,24 +109,9 @@ public: } */ } - - if (0) - { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "kiva_shelf/model.sdf"; - args.m_forceOverrideFixedBase = true; - args.m_fileType = B3_SDF_FILE; - args.m_startOrientation = b3Quaternion(0,0,0,1); - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); - } + { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "plane.urdf"; - args.m_startPosition.setValue(0,0,0); - args.m_forceOverrideFixedBase = true; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); + m_robotSim.loadURDF("plane.urdf"); m_robotSim.setGravity(b3MakeVector3(0,0,0)); } @@ -157,9 +139,7 @@ public: { double q_current[7]={0,0,0,0,0,0,0}; - double world_position[3]={0,0,0}; - double world_orientation[4]={0,0,0,0}; - b3JointStates jointStates; + b3JointStates2 jointStates; if (m_robotSim.getJointStates(m_kukaIndex,jointStates)) { @@ -171,10 +151,13 @@ public: } } // compute body position and orientation - m_robotSim.getLinkState(0, 6, world_position, world_orientation); - m_worldPos.setValue(world_position[0], world_position[1], world_position[2]); - m_worldOri.setValue(world_orientation[0], world_orientation[1], world_orientation[2], world_orientation[3]); - + b3LinkState linkState; + m_robotSim.getLinkState(0, 6, &linkState); + + m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]); + m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]); + + b3Vector3DoubleData targetPosDataOut; m_targetPos.serializeDouble(targetPosDataOut); b3Vector3DoubleData worldPosDataOut; @@ -185,8 +168,8 @@ public: m_worldOri.serializeDouble(worldOriDataOut); - b3RobotSimInverseKinematicArgs ikargs; - b3RobotSimInverseKinematicsResults ikresults; + b3RobotSimulatorInverseKinematicArgs ikargs; + b3RobotSimulatorInverseKinematicsResults ikresults; ikargs.m_bodyUniqueId = m_kukaIndex; @@ -247,7 +230,7 @@ public: //copy the IK result to the desired state of the motor/actuator for (int i=0;i -#include "b3RobotSimAPI.h" +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" #include "../Utils/b3Clock.h" ///quick demo showing the right-handed coordinate system and positive rotations around each axis @@ -19,7 +19,7 @@ class R2D2GraspExample : public CommonExampleInterface { CommonGraphicsApp* m_app; GUIHelperInterface* m_guiHelper; - b3RobotSimAPI m_robotSim; + b3RobotSimulatorClientAPI m_robotSim; int m_options; int m_r2d2Index; @@ -51,20 +51,22 @@ public: } virtual void initPhysics() { - bool connected = m_robotSim.connect(m_guiHelper); + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + b3Printf("robotSim connected = %d",connected); if ((m_options & eROBOTIC_LEARN_GRASP)!=0) { { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "r2d2.urdf"; + b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,0,.5); - b3RobotSimLoadFileResults results; - if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + m_r2d2Index = m_robotSim.loadURDF("r2d2.urdf",args); + + if (m_r2d2Index>=0) { - m_r2d2Index = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(m_r2d2Index); b3Printf("numJoints = %d",numJoints); @@ -78,7 +80,7 @@ public: int wheelTargetVelocities[4]={-10,-10,-10,-10}; for (int i=0;i<4;i++) { - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = wheelTargetVelocities[i]; controlArgs.m_maxTorqueValue = 1e30; m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs); @@ -86,84 +88,67 @@ public: } } { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "kiva_shelf/model.sdf"; - args.m_forceOverrideFixedBase = true; - args.m_fileType = B3_SDF_FILE; - args.m_startOrientation = b3Quaternion(0,0,0,1); - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); + b3RobotSimulatorLoadFileResults results; + m_robotSim.loadSDF("kiva_shelf/model.sdf",results); } { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "plane.urdf"; - args.m_startPosition.setValue(0,0,0); - args.m_forceOverrideFixedBase = true; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.loadURDF("results"); } + + m_robotSim.setGravity(b3MakeVector3(0,0,-10)); } if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0) { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; + b3RobotSimulatorLoadUrdfFileArgs args; + b3RobotSimulatorLoadFileResults results; { - args.m_fileName = "cube_soft.urdf"; args.m_startPosition.setValue(0,0,2.5); args.m_startOrientation.setEulerZYX(0,0.2,0); - m_robotSim.loadFile(args,results); + m_r2d2Index = m_robotSim.loadURDF("cube_soft.urdf",args); } { - args.m_fileName = "cube_no_friction.urdf"; args.m_startPosition.setValue(0,2,2.5); args.m_startOrientation.setEulerZYX(0,0.2,0); - m_robotSim.loadFile(args,results); + m_robotSim.loadURDF("cube_no_friction.urdf",args); } { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "plane.urdf"; args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0.2,0); args.m_forceOverrideFixedBase = true; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.loadURDF("plane.urdf",args); } + + m_robotSim.setGravity(b3MakeVector3(0,0,-10)); } if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0) { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; + b3RobotSimulatorLoadUrdfFileArgs args; + b3RobotSimulatorLoadFileResults results; { - args.m_fileName = "sphere2_rolling_friction.urdf"; args.m_startPosition.setValue(0,0,2.5); args.m_startOrientation.setEulerZYX(0,0,0); args.m_useMultiBody = true; - m_robotSim.loadFile(args,results); + m_robotSim.loadURDF("sphere2_rolling_friction.urdf",args); } { - args.m_fileName = "sphere2.urdf"; args.m_startPosition.setValue(0,2,2.5); args.m_startOrientation.setEulerZYX(0,0,0); args.m_useMultiBody = true; - m_robotSim.loadFile(args,results); + m_robotSim.loadURDF("sphere2.urdf", args); } { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "plane.urdf"; args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0.2,0); args.m_useMultiBody = true; args.m_forceOverrideFixedBase = true; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); - m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + m_robotSim.loadURDF("plane.urdf", args); } - } + + m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + } } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp deleted file mode 100644 index f0e4224ac..000000000 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ /dev/null @@ -1,1063 +0,0 @@ -#include "b3RobotSimAPI.h" - -//#include "../CommonInterfaces/CommonGraphicsAppInterface.h" -#include "Bullet3Common/b3Quaternion.h" -#include "Bullet3Common/b3AlignedObjectArray.h" -#include "../CommonInterfaces/CommonRenderInterface.h" -//#include "../CommonInterfaces/CommonExampleInterface.h" -#include "../SharedMemory/PhysicsServerCommandProcessor.h" - -#include "../CommonInterfaces/CommonGUIHelperInterface.h" -#include "../SharedMemory/PhysicsServerSharedMemory.h" -#include "../SharedMemory/PhysicsServerSharedMemory.h" -#include "../SharedMemory/PhysicsClientC_API.h" -#include "../SharedMemory/PhysicsDirectC_API.h" -#include "../SharedMemory/PhysicsDirect.h" - -#include -#include "../Utils/b3Clock.h" - - -#include "../MultiThreading/b3ThreadSupportInterface.h" - - -void RobotThreadFunc(void* userPtr,void* lsMemory); -void* RobotlsMemoryFunc(); -#define MAX_ROBOT_NUM_THREADS 1 -enum - { - numCubesX = 20, - numCubesY = 20 - }; - - -enum TestRobotSimCommunicationEnums -{ - eRequestTerminateRobotSim= 13, - eRobotSimIsUnInitialized, - eRobotSimIsInitialized, - eRobotSimInitializationFailed, - eRobotSimHasTerminated -}; - -enum MultiThreadedGUIHelperCommunicationEnums -{ - eRobotSimGUIHelperIdle= 13, - eRobotSimGUIHelperRegisterTexture, - eRobotSimGUIHelperRegisterGraphicsShape, - eRobotSimGUIHelperRegisterGraphicsInstance, - eRobotSimGUIHelperCreateCollisionShapeGraphicsObject, - eRobotSimGUIHelperCreateCollisionObjectGraphicsObject, - eRobotSimGUIHelperRemoveAllGraphicsInstances, - eRobotSimGUIHelperCopyCameraImageData, - eRobotSimGUIHelperRemoveGraphicsInstance, -}; - -#include -//#include "BulletMultiThreaded/PlatformDefinitions.h" - -#ifndef _WIN32 -#include "../MultiThreading/b3PosixThreadSupport.h" - -b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads) -{ - b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("RobotSimThreads", - RobotThreadFunc, - RobotlsMemoryFunc, - numThreads); - b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo); - - return threadSupport; - -} - - -#elif defined( _WIN32) -#include "../MultiThreading/b3Win32ThreadSupport.h" - -b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads) -{ - b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("RobotSimThreads",RobotThreadFunc,RobotlsMemoryFunc,numThreads); - b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo); - return threadSupport; - -} -#endif - - - -struct RobotSimArgs -{ - RobotSimArgs() - :m_physicsServerPtr(0) - { - } - b3CriticalSection* m_cs; - - PhysicsServerSharedMemory* m_physicsServerPtr; - b3AlignedObjectArray m_positions; -}; - -struct RobotSimThreadLocalStorage -{ - int threadId; -}; - - -void RobotThreadFunc(void* userPtr,void* lsMemory) -{ - printf("RobotThreadFunc thread started\n"); -// RobotSimThreadLocalStorage* localStorage = (RobotSimThreadLocalStorage*) lsMemory; - - RobotSimArgs* args = (RobotSimArgs*) userPtr; -// int workLeft = true; - b3Clock clock; - clock.reset(); - bool init = true; - if (init) - { - - args->m_cs->lock(); - args->m_cs->setSharedParam(0,eRobotSimIsInitialized); - args->m_cs->unlock(); - - do - { -//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread? -#if 0 - double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.; - if (deltaTimeInSeconds<(1./260.)) - { - if (deltaTimeInSeconds<.001) - continue; - } - - clock.reset(); -#endif // - args->m_physicsServerPtr->processClientCommands(); - - } while (args->m_cs->getSharedParam(0)!=eRequestTerminateRobotSim); - } else - { - args->m_cs->lock(); - args->m_cs->setSharedParam(0,eRobotSimInitializationFailed); - args->m_cs->unlock(); - } - //do nothing -} - - - -void* RobotlsMemoryFunc() -{ - //don't create local store memory, just return 0 - return new RobotSimThreadLocalStorage; -} - - - -ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper2 : public GUIHelperInterface -{ -// CommonGraphicsApp* m_app; - - b3CriticalSection* m_cs; - - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - GUIHelperInterface* m_childGuiHelper; - - const unsigned char* m_texels; - int m_textureWidth; - int m_textureHeight; - - - int m_shapeIndex; - const float* m_position; - const float* m_quaternion; - const float* m_color; - const float* m_scaling; - - const float* m_vertices; - int m_numvertices; - const int* m_indices; - int m_numIndices; - int m_primitiveType; - int m_textureId; - int m_instanceId; - - - MultiThreadedOpenGLGuiHelper2( GUIHelperInterface* guiHelper) - : m_cs(0), - m_texels(0), - m_textureId(-1) - { - m_childGuiHelper = guiHelper;; - - } - - virtual ~MultiThreadedOpenGLGuiHelper2() - { - delete m_childGuiHelper; - } - - void setCriticalSection(b3CriticalSection* cs) - { - m_cs = cs; - } - - b3CriticalSection* getCriticalSection() - { - return m_cs; - } - - virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color) - { - createCollisionObjectGraphicsObject((btCollisionObject*)body, color); - } - - btCollisionObject* m_obj; - btVector3 m_color2; - - virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color) - { - m_obj = obj; - m_color2 = color; - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionObjectGraphicsObject); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - - } - - btCollisionShape* m_colShape; - virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) - { - m_colShape = collisionShape; - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionShapeGraphicsObject); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - - } - - virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) - { - //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects. - //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms - if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0) - { - m_childGuiHelper->syncPhysicsToGraphics(rbWorld); - } - } - - virtual void render(const btDiscreteDynamicsWorld* rbWorld) - { - m_childGuiHelper->render(0); - } - - virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){} - - virtual int registerTexture(const unsigned char* texels, int width, int height) - { - m_texels = texels; - m_textureWidth = width; - m_textureHeight = height; - - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterTexture); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - return m_textureId; - } - virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) - { - m_vertices = vertices; - m_numvertices = numvertices; - m_indices = indices; - m_numIndices = numIndices; - m_primitiveType = primitiveType; - m_textureId = textureId; - - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsShape); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - return m_shapeIndex; - } - virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) - { - m_shapeIndex = shapeIndex; - m_position = position; - m_quaternion = quaternion; - m_color = color; - m_scaling = scaling; - - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsInstance); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - return m_instanceId; - } - - virtual void removeAllGraphicsInstances() - { - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveAllGraphicsInstances); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - } - int m_graphicsInstanceRemove; - virtual void removeGraphicsInstance(int instanceUid) - { - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveGraphicsInstance); - m_graphicsInstanceRemove = instanceUid; - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - } - - virtual Common2dCanvasInterface* get2dCanvasInterface() - { - return 0; - } - - virtual CommonParameterInterface* getParameterInterface() - { - return 0; - } - - virtual CommonRenderInterface* getRenderInterface() - { - return 0; - } - - virtual CommonGraphicsApp* getAppInterface() - { - return m_childGuiHelper->getAppInterface(); - } - - - virtual void setUpAxis(int axis) - { - m_childGuiHelper->setUpAxis(axis); - } - virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ) - { - } - - float m_viewMatrix[16]; - float m_projectionMatrix[16]; - unsigned char* m_pixelsRGBA; - int m_rgbaBufferSizeInPixels; - float* m_depthBuffer; - int m_depthBufferSizeInPixels; - int* m_segmentationMaskBuffer; - int m_segmentationMaskBufferSizeInPixels; - int m_startPixelIndex; - int m_destinationWidth; - int m_destinationHeight; - int* m_numPixelsCopied; - - 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 destinationWidth, - int destinationHeight, int* numPixelsCopied) - { - m_cs->lock(); - for (int i=0;i<16;i++) - { - m_viewMatrix[i] = viewMatrix[i]; - m_projectionMatrix[i] = projectionMatrix[i]; - } - m_pixelsRGBA = pixelsRGBA; - m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels; - m_depthBuffer = depthBuffer; - m_depthBufferSizeInPixels = depthBufferSizeInPixels; - m_segmentationMaskBuffer = segmentationMaskBuffer; - m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels; - m_startPixelIndex = startPixelIndex; - m_destinationWidth = destinationWidth; - m_destinationHeight = destinationHeight; - m_numPixelsCopied = numPixelsCopied; - - m_cs->setSharedParam(1,eRobotSimGUIHelperCopyCameraImageData); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - } - - virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) - { - } - - virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size) - { - } - - virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime) - { - return -1; - } - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ) - { - return -1; - } - virtual void removeUserDebugItem( int debugItemUniqueId) - { - } - virtual void removeAllUserDebugItems( ) - { - } -}; - - - - - -struct b3RobotSimAPI_InternalData -{ - //GUIHelperInterface* m_guiHelper; - PhysicsServerSharedMemory m_physicsServer; - b3PhysicsClientHandle m_physicsClient; - - - b3ThreadSupportInterface* m_threadSupport; - RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS]; - MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper; - PhysicsDirect* m_clientServerDirect; - - bool m_useMultiThreading; - bool m_connected; - - b3RobotSimAPI_InternalData() - : - m_physicsClient(0), - m_threadSupport(0), - m_multiThreadedHelper(0), - - m_clientServerDirect(0), - m_useMultiThreading(false), - m_connected(false) - { - } -}; - -b3RobotSimAPI::b3RobotSimAPI() -{ - m_data = new b3RobotSimAPI_InternalData; -} - -void b3RobotSimAPI::stepSimulation() -{ - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3Assert(b3CanSubmitCommand(m_data->m_physicsClient)); - if (b3CanSubmitCommand(m_data->m_physicsClient)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3InitStepSimulationCommand(m_data->m_physicsClient)); - statusType = b3GetStatusType(statusHandle); - b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED); - } -} - -void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration) -{ - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); - -} - -void b3RobotSimAPI::setNumSolverIterations(int numIterations) -{ - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSolverIterations(command, numIterations); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); - -} - -void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps) -{ - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSubSteps(command, numSubSteps); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); -} - -/* -b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, - const double* jointPositionsQ, double targetPosition[3]); - -int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, - int* bodyUniqueId, - int* dofCount, - double* jointPositions); -*/ -bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results) -{ - btAssert(args.m_endEffectorLinkIndex>=0); - btAssert(args.m_bodyUniqueId>=0); - - - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId); - if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) - { - b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) - { - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); - } else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) - { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } else - { - b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); - } - - if (args.m_flags & B3_HAS_JOINT_DAMPING) - { - b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); - } - - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - - - - int numPos=0; - - bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - 0); - if (result && numPos) - { - results.m_calculatedJointPositions.resize(numPos); - result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - &results.m_calculatedJointPositions[0]); - - } - return result; -} - - -b3RobotSimAPI::~b3RobotSimAPI() -{ - delete m_data; -} - -void b3RobotSimAPI::processMultiThreadedGraphicsRequests() -{ - if (0==m_data->m_multiThreadedHelper) - return; - - switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) - { - case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_data->m_multiThreadedHelper->m_colShape); - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - - break; - } - case eRobotSimGUIHelperCreateCollisionObjectGraphicsObject: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_data->m_multiThreadedHelper->m_obj, - m_data->m_multiThreadedHelper->m_color2); - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperRegisterTexture: - { - - m_data->m_multiThreadedHelper->m_textureId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_data->m_multiThreadedHelper->m_texels, - m_data->m_multiThreadedHelper->m_textureWidth,m_data->m_multiThreadedHelper->m_textureHeight); - - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - - break; - } - case eRobotSimGUIHelperRegisterGraphicsShape: - { - m_data->m_multiThreadedHelper->m_shapeIndex = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape( - m_data->m_multiThreadedHelper->m_vertices, - m_data->m_multiThreadedHelper->m_numvertices, - m_data->m_multiThreadedHelper->m_indices, - m_data->m_multiThreadedHelper->m_numIndices, - m_data->m_multiThreadedHelper->m_primitiveType, - m_data->m_multiThreadedHelper->m_textureId); - - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperRegisterGraphicsInstance: - { - m_data->m_multiThreadedHelper->m_instanceId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance( - m_data->m_multiThreadedHelper->m_shapeIndex, - m_data->m_multiThreadedHelper->m_position, - m_data->m_multiThreadedHelper->m_quaternion, - m_data->m_multiThreadedHelper->m_color, - m_data->m_multiThreadedHelper->m_scaling); - - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperRemoveAllGraphicsInstances: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances(); - int numRenderInstances; - numRenderInstances = m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); - b3Assert(numRenderInstances==0); - - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperRemoveGraphicsInstance: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->removeGraphicsInstance(m_data->m_multiThreadedHelper->m_graphicsInstanceRemove); - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperCopyCameraImageData: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_data->m_multiThreadedHelper->m_viewMatrix, - m_data->m_multiThreadedHelper->m_projectionMatrix, - m_data->m_multiThreadedHelper->m_pixelsRGBA, - m_data->m_multiThreadedHelper->m_rgbaBufferSizeInPixels, - m_data->m_multiThreadedHelper->m_depthBuffer, - m_data->m_multiThreadedHelper->m_depthBufferSizeInPixels, - m_data->m_multiThreadedHelper->m_segmentationMaskBuffer, - m_data->m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels, - m_data->m_multiThreadedHelper->m_startPixelIndex, - m_data->m_multiThreadedHelper->m_destinationWidth, - m_data->m_multiThreadedHelper->m_destinationHeight, - m_data->m_multiThreadedHelper->m_numPixelsCopied); - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - - break; - } - case eRobotSimGUIHelperIdle: - default: - { - - } - } - -} - -b3SharedMemoryStatusHandle b3RobotSimAPI::submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle) -{ - int timeout = 1024*1024*1024; - b3SharedMemoryStatusHandle statusHandle=0; - - b3SubmitClientCommand(physClient,commandHandle); - - while ((statusHandle==0) && (timeout-- > 0)) - { - statusHandle =b3ProcessServerStatus(physClient); - processMultiThreadedGraphicsRequests(); - } - return (b3SharedMemoryStatusHandle) statusHandle; -} - -int b3RobotSimAPI::getNumJoints(int bodyUniqueId) const -{ - return b3GetNumJoints(m_data->m_physicsClient,bodyUniqueId); -} - -bool b3RobotSimAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) -{ - return (b3GetJointInfo(m_data->m_physicsClient,bodyUniqueId, jointIndex,jointInfo)!=0); -} - -void b3RobotSimAPI::createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) -{ - b3SharedMemoryStatusHandle statusHandle; - b3Assert(b3CanSubmitCommand(m_data->m_physicsClient)); - if (b3CanSubmitCommand(m_data->m_physicsClient)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3InitCreateUserConstraintCommand(m_data->m_physicsClient, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); - } -} - - -bool b3RobotSimAPI::getJointStates(int bodyUniqueId, b3JointStates& state) -{ - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - - if (statusHandle) - { - // double rootInertialFrame[7]; - const double* rootLocalInertialFrame; - const double* actualStateQ; - const double* actualStateQdot; - const double* jointReactionForces; - - int stat = b3GetStatusActualState(statusHandle, - &state.m_bodyUniqueId, - &state.m_numDegreeOfFreedomQ, - &state.m_numDegreeOfFreedomU, - &rootLocalInertialFrame, - &actualStateQ, - &actualStateQdot, - &jointReactionForces); - if (stat) - { - state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); - state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); - - for (int i=0;im_physicsClient, bodyUniqueId, CONTROL_MODE_VELOCITY); - b3JointInfo jointInfo; - b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); - int uIndex = jointInfo.m_uIndex; - b3JointControlSetKd(command,uIndex,args.m_kd); - b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); - b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - break; - } - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); - b3JointInfo jointInfo; - b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); - int uIndex = jointInfo.m_uIndex; - int qIndex = jointInfo.m_qIndex; - - b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition); - b3JointControlSetKp(command,uIndex,args.m_kp); - b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); - b3JointControlSetKd(command,uIndex,args.m_kd); - b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - break; - } - case CONTROL_MODE_TORQUE: - { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_TORQUE); - b3JointInfo jointInfo; - b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); - int uIndex = jointInfo.m_uIndex; - b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - break; - } - default: - { - b3Error("Unknown control command in b3RobotSimAPI::setJointMotorControl"); - } - } -} - -bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results) -{ - bool statusOk = false; - - int robotUniqueId = -1; - b3Assert(m_data->m_connected); - switch (args.m_fileType) - { - case B3_URDF_FILE: - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str()); - - //setting the initial position, orientation and other arguments are optional - - b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], - args.m_startPosition[1], - args.m_startPosition[2]); - b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0] - ,args.m_startOrientation[1] - ,args.m_startOrientation[2] - ,args.m_startOrientation[3]); - if (args.m_forceOverrideFixedBase) - { - b3LoadUrdfCommandSetUseFixedBase(command,true); - } - b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); - statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); - statusType = b3GetStatusType(statusHandle); - - b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); - robotUniqueId = b3GetStatusBodyIndex(statusHandle); - results.m_uniqueObjectIds.push_back(robotUniqueId); - statusOk = true; - break; - } - case B3_SDF_FILE: - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str()); - b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); - statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); - statusType = b3GetStatusType(statusHandle); - b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); - if (statusType == CMD_SDF_LOADING_COMPLETED) - { - int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); - if (numBodies) - { - results.m_uniqueObjectIds.resize(numBodies); - int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); - - } - statusOk = true; - } - - break; - } - default: - { - b3Warning("Unknown file type in b3RobotSimAPI::loadFile"); - } - - } - - return statusOk; -} - -bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper) -{ - if (m_data->m_useMultiThreading) - { - m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper); - - - m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); - - for (int i = 0; i < m_data->m_threadSupport->getNumTasks(); i++) - { - RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*)m_data->m_threadSupport->getThreadLocalMemory(i); - b3Assert(storage); - storage->threadId = i; - //storage->m_sharedMem = data->m_sharedMem; - } - - - - - for (int w = 0; w < MAX_ROBOT_NUM_THREADS; w++) - { - m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection(); - m_data->m_args[w].m_cs->setSharedParam(0, eRobotSimIsUnInitialized); - int numMoving = 0; - m_data->m_args[w].m_positions.resize(numMoving); - m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer; - //int index = 0; - - m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*)&m_data->m_args[w], w); - while (m_data->m_args[w].m_cs->getSharedParam(0) == eRobotSimIsUnInitialized) - { - } - } - - m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs); - - bool serverConnected; - serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper); - b3Assert(serverConnected); - - m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); - } - else - { - PhysicsServerCommandProcessor* sdk = new PhysicsServerCommandProcessor; - m_data->m_clientServerDirect = new PhysicsDirect(sdk,true); - bool connected; - connected = m_data->m_clientServerDirect->connect(guiHelper); - m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect; - - } - - //client connected - m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient); - - b3Assert(m_data->m_connected); - return m_data->m_connected && m_data->m_connected; -} - -void b3RobotSimAPI::disconnect() -{ - if (m_data->m_useMultiThreading) - { - for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++) - { - m_data->m_args[i].m_cs->lock(); - m_data->m_args[i].m_cs->setSharedParam(0, eRequestTerminateRobotSim); - m_data->m_args[i].m_cs->unlock(); - } - int numActiveThreads = MAX_ROBOT_NUM_THREADS; - - while (numActiveThreads) - { - int arg0, arg1; - if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0)) - { - numActiveThreads--; - printf("numActiveThreads = %d\n", numActiveThreads); - - } - else - { - } - }; - - printf("stopping threads\n"); - - delete m_data->m_threadSupport; - m_data->m_threadSupport = 0; - } - b3DisconnectSharedMemory(m_data->m_physicsClient); - m_data->m_physicsServer.disconnectSharedMemory(true); - - m_data->m_connected = false; -} - - -void b3RobotSimAPI::debugDraw(int debugDrawMode) -{ - if (m_data->m_clientServerDirect) - { - m_data->m_clientServerDirect->debugDraw(debugDrawMode); - } -} -void b3RobotSimAPI::renderScene() -{ - if (m_data->m_useMultiThreading) - { - if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) - { - m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); - } - } - - - if (m_data->m_clientServerDirect) - { - m_data->m_clientServerDirect->renderScene(); - } - - m_data->m_physicsServer.renderScene(); -} - -void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) -{ - b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClient, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - - if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) - { - b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); - } -} - -void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation) -{ - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - - if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - b3LinkState linkState; - b3GetLinkState(m_data->m_physicsClient, statusHandle, linkIndex, &linkState); - worldPosition[0] = linkState.m_worldPosition[0]; - worldPosition[1] = linkState.m_worldPosition[1]; - worldPosition[2] = linkState.m_worldPosition[2]; - worldOrientation[0] = linkState.m_worldOrientation[0]; - worldOrientation[1] = linkState.m_worldOrientation[1]; - worldOrientation[2] = linkState.m_worldOrientation[2]; - worldOrientation[3] = linkState.m_worldOrientation[3]; - } -} - -void b3RobotSimAPI::loadBunny(double scale, double mass, double collisionMargin) -{ - b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClient); - b3LoadBunnySetScale(command, scale); - b3LoadBunnySetMass(command, mass); - b3LoadBunnySetCollisionMargin(command, collisionMargin); - b3SubmitClientCommand(m_data->m_physicsClient, command); -} diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h deleted file mode 100644 index 7e196e1b6..000000000 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ /dev/null @@ -1,174 +0,0 @@ -#ifndef B3_ROBOT_SIM_API_H -#define B3_ROBOT_SIM_API_H - -///todo: remove those includes from this header -#include "../SharedMemory/PhysicsClientC_API.h" -#include "../SharedMemory/SharedMemoryPublic.h" -#include "Bullet3Common/b3Vector3.h" -#include "Bullet3Common/b3Quaternion.h" -#include "Bullet3Common/b3Transform.h" -#include "Bullet3Common/b3AlignedObjectArray.h" - -#include - -enum b3RigidSimFileType -{ - B3_URDF_FILE=1, - B3_SDF_FILE, - B3_AUTO_DETECT_FILE//todo based on extension -}; - -struct b3JointStates -{ - int m_bodyUniqueId; - int m_numDegreeOfFreedomQ; - int m_numDegreeOfFreedomU; - b3Transform m_rootLocalInertialFrame; - b3AlignedObjectArray m_actualStateQ; - b3AlignedObjectArray m_actualStateQdot; - b3AlignedObjectArray m_jointReactionForces; -}; - -struct b3RobotSimLoadFileArgs -{ - std::string m_fileName; - b3Vector3 m_startPosition; - b3Quaternion m_startOrientation; - bool m_forceOverrideFixedBase; - bool m_useMultiBody; - int m_fileType; - - - b3RobotSimLoadFileArgs(const std::string& fileName) - :m_fileName(fileName), - m_startPosition(b3MakeVector3(0,0,0)), - m_startOrientation(b3Quaternion(0,0,0,1)), - m_forceOverrideFixedBase(false), - m_useMultiBody(true), - m_fileType(B3_URDF_FILE) - { - } -}; - - -struct b3RobotSimLoadFileResults -{ - b3AlignedObjectArray m_uniqueObjectIds; - b3RobotSimLoadFileResults() - { - } -}; - -struct b3JointMotorArgs -{ - int m_controlMode; - - double m_targetPosition; - double m_kp; - - double m_targetVelocity; - double m_kd; - - double m_maxTorqueValue; - - b3JointMotorArgs(int controlMode) - :m_controlMode(controlMode), - m_targetPosition(0), - m_kp(0.1), - m_targetVelocity(0), - m_kd(0.9), - m_maxTorqueValue(1000) - { - } -}; - -enum b3InverseKinematicsFlags -{ - B3_HAS_IK_TARGET_ORIENTATION=1, - B3_HAS_NULL_SPACE_VELOCITY=2, - B3_HAS_JOINT_DAMPING=4, -}; - -struct b3RobotSimInverseKinematicArgs -{ - int m_bodyUniqueId; -// double* m_currentJointPositions; -// int m_numPositions; - double m_endEffectorTargetPosition[3]; - double m_endEffectorTargetOrientation[4]; - int m_endEffectorLinkIndex; - int m_flags; - int m_numDegreeOfFreedom; - b3AlignedObjectArray m_lowerLimits; - b3AlignedObjectArray m_upperLimits; - b3AlignedObjectArray m_jointRanges; - b3AlignedObjectArray m_restPoses; - b3AlignedObjectArray m_jointDamping; - - b3RobotSimInverseKinematicArgs() - :m_bodyUniqueId(-1), - m_endEffectorLinkIndex(-1), - m_flags(0) - { - m_endEffectorTargetPosition[0]=0; - m_endEffectorTargetPosition[1]=0; - m_endEffectorTargetPosition[2]=0; - - m_endEffectorTargetOrientation[0]=0; - m_endEffectorTargetOrientation[1]=0; - m_endEffectorTargetOrientation[2]=0; - m_endEffectorTargetOrientation[3]=1; - } -}; - -struct b3RobotSimInverseKinematicsResults -{ - int m_bodyUniqueId; - b3AlignedObjectArray m_calculatedJointPositions; -}; - -class b3RobotSimAPI -{ - struct b3RobotSimAPI_InternalData* m_data; - void processMultiThreadedGraphicsRequests(); - b3SharedMemoryStatusHandle submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle); - -public: - b3RobotSimAPI(); - virtual ~b3RobotSimAPI(); - - bool connect(struct GUIHelperInterface* guiHelper); - void disconnect(); - - bool loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results); - - int getNumJoints(int bodyUniqueId) const; - - bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); - - void createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); - - bool getJointStates(int bodyUniqueId, b3JointStates& state); - - void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args); - - void stepSimulation(); - - void setGravity(const b3Vector3& gravityAcceleration); - - void setNumSimulationSubSteps(int numSubSteps); - void setNumSolverIterations(int numIterations); - - bool calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results); - - void renderScene(); - void debugDraw(int debugDrawMode); - - void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); - - void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation); - - void loadBunny(double scale, double mass, double collisionMargin); -}; - -#endif //B3_ROBOT_SIM_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f75e2c5e7..7a353bbf7 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2320,9 +2320,30 @@ b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physCl command->m_updateVisualShapeDataArguments.m_shapeIndex = shapeIndex; command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId; command->m_updateFlags = 0; + + if (textureUniqueId>=0) + { + command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_TEXTURE; + } return (b3SharedMemoryCommandHandle) command; } +void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE); + + if (command->m_type == CMD_UPDATE_VISUAL_SHAPE) + { + command->m_updateVisualShapeDataArguments.m_rgbaColor[0] = rgbaColor[0]; + command->m_updateVisualShapeDataArguments.m_rgbaColor[1] = rgbaColor[1]; + command->m_updateVisualShapeDataArguments.m_rgbaColor[2] = rgbaColor[2]; + command->m_updateVisualShapeDataArguments.m_rgbaColor[3] = rgbaColor[3]; + command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR; + } +} + b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 382e144d8..b9802de36 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -198,6 +198,7 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); +void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]); b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index b436a9fef..f44e438e8 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -14,7 +14,7 @@ protected: void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); void resetData(); void removeCachedBody(int bodyUniqueId); - + virtual void renderSceneInternal() {}; public: PhysicsClientSharedMemory(); virtual ~PhysicsClientSharedMemory(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4162cab19..9b3c724c2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5585,9 +5585,52 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; - m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); - - serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId>=0) + { + m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); + } + } + + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) + { + int bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId; + int linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex; + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (bodyHandle) + { + if (bodyHandle->m_multiBody) + { + if (linkIndex==-1) + { + if (bodyHandle->m_multiBody->getBaseCollider()) + { + //m_data->m_visualConverter.changeRGBAColor(...) + int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + } else + { + if (linkIndexm_multiBody->getNumLinks()) + { + if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) + { + //m_data->m_visualConverter.changeRGBAColor(...) + int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); + m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + } + } + } else + { + //todo: change color for rigid body + } + } + } + + serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; hasStatus = true; break; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index d0f6269a5..7ffa48b21 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -180,6 +180,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIUserDebugRemoveAllItems, eGUIDumpFramesToVideo, eGUIHelperRemoveGraphicsInstance, + eGUIHelperChangeGraphicsInstanceRGBAColor, }; @@ -788,6 +789,20 @@ public: workerThreadWait(); } + double m_rgbaColor[4]; + int m_graphicsInstanceChangeColor; + virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) + { + m_graphicsInstanceChangeColor = instanceUid; + m_rgbaColor[0] = rgbaColor[0]; + m_rgbaColor[1] = rgbaColor[1]; + m_rgbaColor[2] = rgbaColor[2]; + m_rgbaColor[3] = rgbaColor[3]; + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperChangeGraphicsInstanceRGBAColor); + workerThreadWait(); + } + virtual Common2dCanvasInterface* get2dCanvasInterface() { return 0; @@ -1573,8 +1588,13 @@ void PhysicsServerExample::updateGraphics() m_multiThreadedHelper->mainThreadRelease(); break; } - - + + case eGUIHelperChangeGraphicsInstanceRGBAColor: + { + m_multiThreadedHelper->m_childGuiHelper->changeRGBAColor(m_multiThreadedHelper->m_graphicsInstanceChangeColor,m_multiThreadedHelper->m_rgbaColor); + m_multiThreadedHelper->mainThreadRelease(); + break; + } case eGUIHelperCopyCameraImageData: { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index df9dbc14e..8b1feed62 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -245,12 +245,19 @@ struct RequestVisualShapeDataArgs int m_startingVisualShapeIndex; }; +enum EnumUpdateVisualShapeData +{ + CMD_UPDATE_VISUAL_SHAPE_TEXTURE=1, + CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR=2, +}; + struct UpdateVisualShapeDataArgs { int m_bodyUniqueId; int m_jointIndex; int m_shapeIndex; int m_textureUniqueId; + double m_rgbaColor[4]; }; struct LoadTextureArgs diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 476b33fb7..70fd1c5fb 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -4,6 +4,9 @@ #include "PhysicsClientSharedMemory.h" #include"../ExampleBrowser/InProcessExampleBrowser.h" +#include "PhysicsServerExample.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "InProcessMemory.h" #include "Bullet3Common/b3Logging.h" class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory @@ -124,3 +127,109 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a return (b3PhysicsClientHandle ) cl; } +class InProcessPhysicsClientExistingExampleBrowser : public PhysicsClientSharedMemory +{ + CommonExampleInterface* m_physicsServerExample; + SharedMemoryInterface* m_sharedMem; + b3Clock m_clock; + unsigned long long int m_prevTime; +public: + InProcessPhysicsClientExistingExampleBrowser (struct GUIHelperInterface* guiHelper) + { + + + m_sharedMem = new InProcessMemory; + CommonExampleOptions options(guiHelper); + options.m_sharedMem = m_sharedMem; + + m_physicsServerExample = PhysicsServerCreateFunc(options); + m_physicsServerExample ->initPhysics(); + m_physicsServerExample ->resetCamera(); + setSharedMemoryInterface(m_sharedMem); + m_clock.reset(); + m_prevTime = m_clock.getTimeMicroseconds(); + + } + virtual ~InProcessPhysicsClientExistingExampleBrowser() + { + m_physicsServerExample->exitPhysics(); + //s_instancingRenderer->removeAllInstances(); + delete m_physicsServerExample; + delete m_sharedMem; + } + + // return non-null if there is a status, nullptr otherwise + virtual const struct SharedMemoryStatus* processServerStatus() + { + //m_physicsServerExample->updateGraphics(); + + unsigned long long int curTime = m_clock.getTimeMicroseconds(); + unsigned long long int dtMicro = curTime - m_prevTime; + m_prevTime = curTime; + + double dt = double(dtMicro)/1000000.; + + m_physicsServerExample->stepSimulation(dt); + { + b3Clock::usleep(0); + } + const SharedMemoryStatus* stat = 0; + + { + stat = PhysicsClientSharedMemory::processServerStatus(); + } + + return stat; + + } + + virtual void renderScene() + { + m_physicsServerExample->renderScene(); + } + virtual void debugDraw(int debugDrawMode) + { + m_physicsServerExample->physicsDebugDraw(debugDrawMode); + } + virtual bool mouseMoveCallback(float x, float y) + { + return m_physicsServerExample->mouseMoveCallback(x,y); + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return m_physicsServerExample->mouseButtonCallback(button,state,x,y); + } + +}; + +void b3InProcessDebugDrawInternal(b3PhysicsClientHandle clientHandle, int debugDrawMode) +{ + InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle; + cl->debugDraw(debugDrawMode); +} +void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle) +{ + InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle; + cl->renderScene(); +} + +int b3InProcessMouseMoveCallback(b3PhysicsClientHandle clientHandle,float x,float y) +{ + InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle; + return cl->mouseMoveCallback(x,y); +} +int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int button, int state, float x, float y) +{ + InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle; + return cl->mouseButtonCallback(button,state, x,y); +} + + +b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper) +{ + + InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper); + //InProcessPhysicsClientFromGuiHelper* cl = new InProcessPhysicsClientFromGuiHelper(guiHelper); + cl->connect(); + return (b3PhysicsClientHandle ) cl; +} \ No newline at end of file diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h index 7e68a64f8..6d1bacc75 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h @@ -14,9 +14,14 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]); - - - +b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper); + +///ignore the following APIs, they are for internal use for example browser +void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle); +void b3InProcessDebugDrawInternal(b3PhysicsClientHandle clientHandle, int debugDrawMode); +int b3InProcessMouseMoveCallback(b3PhysicsClientHandle clientHandle,float x,float y); +int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int button, int state, float x, float y); + #ifdef __cplusplus } #endif diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index eccda8cfc..f451e7a79 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -503,6 +503,7 @@ enum eCONNECT_METHOD { eCONNECT_SHARED_MEMORY = 3, eCONNECT_UDP = 4, eCONNECT_TCP = 5, + eCONNECT_EXISTING_EXAMPLE_BROWSER=6, }; enum eURDF_Flags diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 5a74d2ad8..db7bcbb35 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -487,7 +487,10 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) Model* model = renderData.m_model; if (0==model) return; - + //discard invisible objects (zero alpha) + if (model->getColorRGBA()[3]==0) + return; + renderData.m_viewportMatrix = viewport(0,0,width, height); b3AlignedObjectArray& zbuffer = renderData.m_depthBuffer; @@ -506,7 +509,8 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) btVector3 P(viewMatrixInv[0][3], viewMatrixInv[1][3], viewMatrixInv[2][3]); Shader shader(model, light_dir_local, light_color, modelViewMatrix, lightModelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, renderData.m_viewportMatrix, localScaling, model->getColorRGBA(), width, height, shadowBufferPtr, renderData.m_lightAmbientCoeff, renderData.m_lightDiffuseCoeff, renderData.m_lightSpecularCoeff); - + + for (int i=0; infaces(); i++) { B3_PROFILE("face"); diff --git a/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp index 82014e630..bcf7d13ba 100644 --- a/examples/Utils/b3Clock.cpp +++ b/examples/Utils/b3Clock.cpp @@ -198,11 +198,13 @@ void b3Clock::usleep(int microSeconds) Sleep(millis); } #else - - ::usleep(microSeconds); - //struct timeval tv; - //tv.tv_sec = microSeconds/1000000L; - //tv.tv_usec = microSeconds%1000000L; - //return select(0, 0, 0, 0, &tv); + if (microSeconds>0) + { + ::usleep(microSeconds); + //struct timeval tv; + //tv.tv_sec = microSeconds/1000000L; + //tv.tv_usec = microSeconds%1000000L; + //return select(0, 0, 0, 0, &tv); + } #endif } diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 995110b81..6b6004642 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4027,7 +4027,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO return Py_None; } -static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_changeVisualShape(PyObject* self, PyObject* args, PyObject* keywds) { int objectUniqueId = -1; int jointIndex = -1; @@ -4037,9 +4037,11 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args, P b3SharedMemoryStatusHandle statusHandle; int statusType; int physicsClientId = 0; + PyObject* rgbaColorObj = 0; + b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"objectUniqueId", "jointIndex", "shapeIndex", "textureUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiii|i", kwlist, &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId, &physicsClientId)) + static char* kwlist[] = {"objectUniqueId", "linkIndex", "shapeIndex", "textureUniqueId", "rgbaColor", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|iiOi", kwlist, &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId, &rgbaColorObj, &physicsClientId)) { return NULL; } @@ -4052,6 +4054,14 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args, P { commandHandle = b3InitUpdateVisualShape(sm, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); + + if (rgbaColorObj) + { + double rgbaColor[4] = {1,1,1,1}; + pybullet_internalSetVector4d(rgbaColorObj, rgbaColor); + b3UpdateVisualShapeRGBAColor(commandHandle,rgbaColor); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED) @@ -5913,7 +5923,7 @@ static PyMethodDef SpamMethods[] = { "Reset the state (position, velocity etc) for a joint on a body " "instantaneously, not through physics simulation."}, - {"changeDynamicsInfo", (PyCFunction)pybullet_changeDynamicsInfo, METH_VARARGS | METH_KEYWORDS, + {"changeDynamics", (PyCFunction)pybullet_changeDynamicsInfo, METH_VARARGS | METH_KEYWORDS, "change dynamics information such as mass, lateral friction coefficient."}, {"getDynamicsInfo", (PyCFunction)pybullet_getDynamicsInfo, METH_VARARGS | METH_KEYWORDS, @@ -6019,8 +6029,11 @@ static PyMethodDef SpamMethods[] = { {"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS | METH_KEYWORDS, "Return the visual shape information for one object."}, - {"resetVisualShapeData", (PyCFunction)pybullet_resetVisualShapeData, METH_VARARGS | METH_KEYWORDS, - "Reset part of the visual shape information for one object."}, + {"changeVisualShape", (PyCFunction)pybullet_changeVisualShape, METH_VARARGS | METH_KEYWORDS, + "Change part of the visual shape information for one object."}, + + {"resetVisualShapeData", (PyCFunction)pybullet_changeVisualShape, METH_VARARGS | METH_KEYWORDS, + "Obsolete method, kept for backward compatibility, use changeVisualShapeData instead."}, {"loadTexture", (PyCFunction)pybullet_loadTexture, METH_VARARGS | METH_KEYWORDS, "Load texture file."}, diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index f4e3fccb5..6f8c6df9d 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -204,6 +204,8 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Utils/RobotLoggingUtil.h", "../../examples/Utils/b3Clock.cpp", "../../examples/Utils/b3Clock.h", + "../../examples/Utils/ChromeTraceUtil.cpp", + "../../examples/Utils/ChromeTraceUtil.h", "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",