From b880ddf76bd45f78f43517a139b99e6908074a37 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 8 Aug 2016 14:23:44 -0700 Subject: [PATCH 1/2] add pybullet render API with yaw/pitch/roll option add testrender.py file allow option to enable OpenGL hardware renderer in multithreaded sim b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL); --- build_and_run_cmake.sh | 2 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 2 +- examples/RoboticsLearning/b3RobotSimAPI.cpp | 64 ++++++++++-- examples/SharedMemory/PhysicsClientC_API.cpp | 99 ++++++++++++------- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../SharedMemory/PhysicsClientExample.cpp | 12 +-- .../PhysicsClientSharedMemory.cpp | 2 +- examples/SharedMemory/PhysicsDirect.cpp | 4 +- .../SharedMemory/PhysicsServerExample.cpp | 55 ++++++++++- examples/pybullet/pybullet.c | 12 ++- examples/pybullet/testrender.py | 46 +++++---- 11 files changed, 219 insertions(+), 81 deletions(-) diff --git a/build_and_run_cmake.sh b/build_and_run_cmake.sh index 28354357b..13cb76c62 100755 --- a/build_and_run_cmake.sh +++ b/build_and_run_cmake.sh @@ -2,6 +2,6 @@ rm CMakeCache.txt mkdir build_cmake cd build_cmake -cmake -DBUILD_PYTHON=OFF -CMAKE_BUILD_TYPE=Release .. +cmake -DBUILD_PYBULLET=OFF -DCMAKE_BUILD_TYPE=Release .. make -j12 examples/ExampleBrowser/App_ExampleBrowser diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index d0fae2254..d6fab65a3 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -520,7 +520,7 @@ void MyStatusBarError(const char* msg) gui2->textOutput(msg); gui2->forceUpdateScrollBars(); } - btAssert(0); + btAssert(0); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index ce9f032b7..38914873e 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -47,6 +47,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eRobotSimGUIHelperCreateCollisionShapeGraphicsObject, eRobotSimGUIHelperCreateCollisionObjectGraphicsObject, eRobotSimGUIHelperRemoveAllGraphicsInstances, + eRobotSimGUIHelperCopyCameraImageData, }; #include @@ -152,7 +153,7 @@ void* RobotlsMemoryFunc() -ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper : public GUIHelperInterface +ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper2 : public GUIHelperInterface { CommonGraphicsApp* m_app; @@ -185,7 +186,7 @@ public: int m_instanceId; - MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) + MultiThreadedOpenGLGuiHelper2(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) :m_app(app) ,m_cs(0), m_texels(0), @@ -195,7 +196,7 @@ public: } - virtual ~MultiThreadedOpenGLGuiHelper() + virtual ~MultiThreadedOpenGLGuiHelper2() { delete m_childGuiHelper; } @@ -345,10 +346,39 @@ public: { } + float m_viewMatrix[16]; + float m_projectionMatrix[16]; + unsigned char* m_pixelsRGBA; + int m_rgbaBufferSizeInPixels; + float* m_depthBuffer; + int m_depthBufferSizeInPixels; + 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 startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied) { - if (numPixelsCopied) - *numPixelsCopied = 0; + 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_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) @@ -375,7 +405,7 @@ struct b3RobotSimAPI_InternalData b3ThreadSupportInterface* m_threadSupport; RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS]; - MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper; + MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper; bool m_connected; @@ -494,6 +524,24 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests() 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_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: { @@ -669,9 +717,9 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper) { - m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper); + m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); - MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper); + MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 23dc57826..fbb773854 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -825,7 +825,7 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } -void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, int upAxis) +void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -834,47 +834,78 @@ void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandl b3Vector3 camForward; b3Vector3 camPos; b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0],cameraTargetPosition[1],cameraTargetPosition[2]); - + b3Vector3 eyePos = b3MakeVector3(0,0,0); + int forwardAxis(-1); + + { + switch (upAxis) { - case 1: - forwardAxis = 2; - camUpVector = b3MakeVector3(0,1,0); - //gLightPos = b3MakeVector3(-50.f,100,30); - break; - case 2: - forwardAxis = 1; - camUpVector = b3MakeVector3(0,0,1); - //gLightPos = b3MakeVector3(-50.f,30,100); - break; - default: - { - //b3Assert(0); - return; - } - }; - - b3Vector3 eyePos = b3MakeVector3(0,0,0); - eyePos[forwardAxis] = -distance; - - camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]); - if (camForward.length2() < B3_EPSILON) - { - camForward.setValue(1.f,0.f,0.f); - } else - { - camForward.normalize(); + + case 1: + { + + + forwardAxis = 0; + eyePos[forwardAxis] = -distance; + camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]); + if (camForward.length2() < B3_EPSILON) + { + camForward.setValue(1.f,0.f,0.f); + } else + { + camForward.normalize(); + } + b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547); + b3Quaternion rollRot(camForward,rollRad); + + camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,1,0)); + //gLightPos = b3MakeVector3(-50.f,100,30); + break; + } + case 2: + { + + + forwardAxis = 1; + eyePos[forwardAxis] = -distance; + camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]); + if (camForward.length2() < B3_EPSILON) + { + camForward.setValue(1.f,0.f,0.f); + } else + { + camForward.normalize(); + } + + b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547); + b3Quaternion rollRot(camForward,rollRad); + + camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,0,1)); + //gLightPos = b3MakeVector3(-50.f,30,100); + break; + } + default: + { + //b3Assert(0); + return; + } + }; } + - b3Scalar rele = yaw * b3Scalar(0.01745329251994329547);// rads per deg - b3Scalar razi = pitch * b3Scalar(0.01745329251994329547);// rads per deg - b3Quaternion rot(camUpVector,razi); + b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547);// rads per deg + b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547);// rads per deg + + b3Quaternion pitchRot(camUpVector,pitchRad); b3Vector3 right = camUpVector.cross(camForward); - b3Quaternion roll(right,-rele); + b3Quaternion yawRot(right,-yawRad); + + - eyePos = b3Matrix3x3(rot) * b3Matrix3x3(roll) * eyePos; + eyePos = b3Matrix3x3(pitchRot) * b3Matrix3x3(yawRot) * eyePos; camPos = eyePos; camPos += camTargetPos; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 588f810a8..f55876fbd 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -71,7 +71,7 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]); -void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, int upAxis); +void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis); void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal); void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal); void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index b5788bd09..017e67a53 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -259,15 +259,15 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle); //b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL); - float viewMatrix[16]; - float projectionMatrix[16]; - m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); + float viewMatrix[16]; + float projectionMatrix[16]; + m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix); - b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); - break; + b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index fa4362caf..fc807fabb 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -609,7 +609,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { { SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; - if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0) + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 511290778..2f1b1f413 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -225,7 +225,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) = rgbaPixelsReceived[i]; } - if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0) + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) { @@ -241,7 +241,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; } } - } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0); + } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied); return m_data->m_hasStatus; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 75e475738..53ef9ca31 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -43,6 +43,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperCreateCollisionShapeGraphicsObject, eGUIHelperCreateCollisionObjectGraphicsObject, eGUIHelperRemoveAllGraphicsInstances, + eGUIHelperCopyCameraImageData, }; #include @@ -392,11 +393,41 @@ public: { } - virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int width, int height, int* numPixelsCopied) + float m_viewMatrix[16]; + float m_projectionMatrix[16]; + unsigned char* m_pixelsRGBA; + int m_rgbaBufferSizeInPixels; + float* m_depthBuffer; + int m_depthBufferSizeInPixels; + 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 startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied) { - if (numPixelsCopied) - *numPixelsCopied = 0; + 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_startPixelIndex = startPixelIndex; + m_destinationWidth = destinationWidth; + m_destinationHeight = destinationHeight; + m_numPixelsCopied = numPixelsCopied; + + m_cs->setSharedParam(1,eGUIHelperCopyCameraImageData); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eGUIHelperIdle) + { + } } + virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) { @@ -727,6 +758,24 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->getCriticalSection()->unlock(); break; } + + case eGUIHelperCopyCameraImageData: + { + m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix, + m_multiThreadedHelper->m_projectionMatrix, + m_multiThreadedHelper->m_pixelsRGBA, + m_multiThreadedHelper->m_rgbaBufferSizeInPixels, + m_multiThreadedHelper->m_depthBuffer, + m_multiThreadedHelper->m_depthBufferSizeInPixels, + m_multiThreadedHelper->m_startPixelIndex, + m_multiThreadedHelper->m_destinationWidth, + m_multiThreadedHelper->m_destinationHeight, + m_multiThreadedHelper->m_numPixelsCopied); + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } case eGUIHelperIdle: default: { diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ba34093e8..e00c5f0af 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1154,20 +1154,21 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal); } } - else if (size==10) + else if (size==11) { int upAxisIndex=1; - float camDistance,yaw,pitch; + float camDistance,yaw,pitch,roll; //sometimes more arguments are better :-) - if (PyArg_ParseTuple(args, "iiOfffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &upAxisIndex, &nearVal, &farVal, &fov)) + if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &roll, &upAxisIndex, &nearVal, &farVal, &fov)) { + b3RequestCameraImageSetPixelResolution(command,width,height); if (pybullet_internalSetVector(objTargetPos, targetPos)) { //printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); - b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,upAxisIndex); + b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,roll,upAxisIndex); aspect = width/height; b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal); } else @@ -1193,6 +1194,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { b3SharedMemoryStatusHandle statusHandle; int statusType; + + //b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType==CMD_CAMERA_IMAGE_COMPLETED) diff --git a/examples/pybullet/testrender.py b/examples/pybullet/testrender.py index 7ba07e4e7..da96bbbc5 100644 --- a/examples/pybullet/testrender.py +++ b/examples/pybullet/testrender.py @@ -6,14 +6,16 @@ pybullet.connect(pybullet.GUI) pybullet.loadURDF("r2d2.urdf") camTargetPos = [0,0,0] -#cameraUp = [0,0,1] -cameraPos = [3,3,3] -yaw = 40.0 -pitch = 0.0 -upAxisIndex = 1 -camDistance = 3 -pixelWidth = 640 -pixelHeight = 480 +cameraUp = [0,0,1] +cameraPos = [1,1,1] +yaw = 40 +pitch = 10.0 + +roll=0 +upAxisIndex = 2 +camDistance = 4 +pixelWidth = 320 +pixelHeight = 240 nearPlane = 0.01 farPlane = 1000 @@ -22,19 +24,23 @@ fov = 60 #img_arr = pybullet.renderImage(pixelWidth, pixelHeight) #renderImage(w, h, view[16], projection[16]) #img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane) -img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, upAxisIndex, nearPlane, farPlane, fov) +for pitch in range (0,360,10) : + img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex, nearPlane, farPlane, fov) -w=img_arr[0] #width of the image, in pixels -h=img_arr[1] #height of the image, in pixels -rgb=img_arr[2] #color data RGB -dep=img_arr[3] #depth data + w=img_arr[0] #width of the image, in pixels + h=img_arr[1] #height of the image, in pixels + rgb=img_arr[2] #color data RGB + dep=img_arr[3] #depth data + #print 'width = %d height = %d' % (w,h) -# reshape creates np array -np_img_arr = np.reshape(rgb, (pixelHeight, pixelWidth, 4)) -np_img_arr = np_img_arr*(1./255.) + # reshape creates np array + np_img_arr = np.reshape(rgb, (h, w, 4)) + np_img_arr = np_img_arr*(1./255.) -#show -plt.imshow(np_img_arr,interpolation='none') -plt.show() -p.resetSimulation() + #show + plt.imshow(np_img_arr,interpolation='none') + #plt.show() + plt.pause(0.01) + +pybullet.resetSimulation() From a9b1544a9f3a30f8537933f81f5a3c601a3cc41e Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 9 Aug 2016 18:40:12 -0700 Subject: [PATCH 2/2] Add premake support to build pybullet, Windows and Linux tested, will enable Mac in next commit. Expose inverse dynamics to Bullet shared memory API, through b3CalculateInverseDynamicsCommandInit and b3GetStatusInverseDynamicsJointForces command/status. See PhysicsClientExeample or pybullet for usage. Add option for Windows and Linux to set python_lib_dir and python_include_dir for premake and --enable_pybullet option Expose inverse dynamics to pybullet: [force] = p.calculateInverseDynamics(objectIndex,[q],[qdot],[acc]) Thanks to Jeff Bingham for the suggestion. --- build3/premake4.lua | 40 +++++- examples/SharedMemory/PhysicsClientC_API.cpp | 58 +++++++++ examples/SharedMemory/PhysicsClientC_API.h | 10 ++ .../SharedMemory/PhysicsClientExample.cpp | 58 +++++++++ .../PhysicsClientSharedMemory.cpp | 10 +- .../PhysicsServerCommandProcessor.cpp | 95 +++++++++++++- .../PhysicsServerCommandProcessor.h | 2 + examples/SharedMemory/SharedMemoryCommands.h | 19 +++ examples/SharedMemory/SharedMemoryPublic.h | 3 + examples/SharedMemory/premake4.lua | 6 +- examples/pybullet/CMakeLists.txt | 109 ++++++++-------- examples/pybullet/premake4.lua | 68 +++++++++- examples/pybullet/pybullet.c | 121 +++++++++++++++++- test/SharedMemory/CMakeLists.txt | 2 +- test/SharedMemory/premake4.lua | 92 ++++++------- 15 files changed, 575 insertions(+), 118 deletions(-) diff --git a/build3/premake4.lua b/build3/premake4.lua index 862dfc2a5..744a55214 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -85,12 +85,36 @@ newoption { - trigger = "python", - description = "Enable Python scripting (experimental, use Physics Server in Example Browser). " + trigger = "enable_pybullet", + description = "Enable high-level Python scripting of Bullet with URDF/SDF import and synthetic camera." } +if os.is("Linux") then + default_python_include_dir = "/usr/include/python2.7" + default_python_lib_dir = "/usr/local/lib/" +end + +if os.is("Windows") then + default_python_include_dir = "C:/Python34/include" + default_python_lib_dir = "C:/Python34/libs" +end + newoption + { + trigger = "python_include_dir", + value = default_python_include_dir, + description = "Python (2.x or 3.x) include directory" + } + + newoption + { + trigger = "python_lib_dir", + value = default_python_lib_dir, + description = "Python (2.x or 3.x) library directory " + } + + newoption { trigger = "targetdir", value = "path such as ../bin", @@ -140,7 +164,7 @@ platforms {"x32"} end else - platforms {"x32", "x64"} + platforms {"x64"} end configuration {"x32"} @@ -191,6 +215,14 @@ targetdir( _OPTIONS["targetdir"] or "../bin" ) location("./" .. act .. postfix) + if not _OPTIONS["python_include_dir"] then + _OPTIONS["python_include_dir"] = default_python_include_dir + end + + if not _OPTIONS["python_lib_dir"] then + _OPTIONS["python_lib_dir"] = default_python_lib_dir + end + projectRootDir = os.getcwd() .. "/../" print("Project root directory: " .. projectRootDir); @@ -222,7 +254,7 @@ if _OPTIONS["lua"] then include "../examples/ThirdPartyLibs/lua-5.2.3" end - if _OPTIONS["python"] then + if _OPTIONS["enable_pybullet"] then include "../examples/pybullet" end diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index fbb773854..8530c0c5a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -626,6 +626,7 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) return bodyId; } + int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* numDegreeOfFreedomQ, @@ -1097,3 +1098,60 @@ void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUn command->m_externalForceArguments.m_numForcesAndTorques++; } + + + +///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics +b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, + const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS; + command->m_updateFlags = 0; + command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex; + int numJoints = cl->getNumJoints(bodyIndex); + for (int i = 0; i < numJoints;i++) + { + command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; + command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i]; + command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i]; + } + + return (b3SharedMemoryCommandHandle)command; +} + +int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, + int* bodyUniqueId, + int* dofCount, + double* jointForces) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED); + if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) + return false; + + + if (dofCount) + { + *dofCount = status->m_inverseDynamicsResultArgs.m_dofCount; + } + if (bodyUniqueId) + { + *bodyUniqueId = status->m_inverseDynamicsResultArgs.m_bodyUniqueId; + } + if (jointForces) + { + for (int i = 0; i < status->m_inverseDynamicsResultArgs.m_dofCount; i++) + { + jointForces[i] = status->m_inverseDynamicsResultArgs.m_jointForces[i]; + } + } + + + return true; +} \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index f55876fbd..6f7b13647 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -97,6 +97,16 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); +///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics +b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, + const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); + +int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, + int* bodyUniqueId, + int* dofCount, + double* jointForces); + + b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); ///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 017e67a53..801dd0d89 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -413,6 +413,33 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } + case CMD_CALCULATE_INVERSE_DYNAMICS: + { + if (m_selectedBody >= 0) + { + btAlignedObjectArray jointPositionsQ; + btAlignedObjectArray jointVelocitiesQdot; + btAlignedObjectArray jointAccelerations; + int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); + if (numJoints) + { + b3Printf("Compute inverse dynamics for joint accelerations:"); + jointPositionsQ.resize(numJoints); + jointVelocitiesQdot.resize(numJoints); + jointAccelerations.resize(numJoints); + for (int i = 0; i < numJoints; i++) + { + jointAccelerations[i] = 100; + b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]); + } + b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle, + m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + + } + } + break; + } default: { b3Error("Unknown buttonId"); @@ -490,6 +517,7 @@ void PhysicsClientExample::createButtons() createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger); createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); + createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger); if (m_bodyUniqueIds.size()) { @@ -695,6 +723,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime) // b3Printf(msg); } + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) + { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(status, + &bodyUniqueId, + &dofCount, + 0); + + btAlignedObjectArray jointForces; + if (dofCount) + { + jointForces.resize(dofCount); + b3GetStatusInverseDynamicsJointForces(status, + 0, + 0, + &jointForces[0]); + for (int i = 0; i < dofCount; i++) + { + b3Printf("jointForces[%d]=%f", i, jointForces[i]); + } + } + + } + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_FAILED) + { + b3Warning("Inverse Dynamics computations failed"); + } + if (statusType == CMD_CAMERA_IMAGE_FAILED) { b3Warning("Camera image FAILED\n"); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index fc807fabb..433515091 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -542,7 +542,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Camera image FAILED\n"); break; } - + case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED: + { + break; + } + case CMD_CALCULATED_INVERSE_DYNAMICS_FAILED: + { + b3Warning("Inverse Dynamics computations failed"); + break; + } default: { b3Error("Unknown server status\n"); btAssert(0); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8a54ba184..1937448ce 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4,13 +4,15 @@ #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" #include "TinyRendererVisualShapeConverter.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" - +#include "LinearMath/btHashMap.h" +#include "BulletInverseDynamics/MultiBodyTree.hpp" #include "btBulletDynamicsCommon.h" @@ -383,6 +385,7 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_physicsDeltaTime; btAlignedObjectArray m_multiBodyJointFeedbacks; + btHashMap m_inverseDynamicsBodies; @@ -548,8 +551,25 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); } +void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies() +{ + for (int i = 0; i < m_data->m_inverseDynamicsBodies.size(); i++) + { + btInverseDynamics::MultiBodyTree** treePtrPtr = m_data->m_inverseDynamicsBodies.getAtIndex(i); + if (treePtrPtr) + { + btInverseDynamics::MultiBodyTree* tree = *treePtrPtr; + delete tree; + } + + } + m_data->m_inverseDynamicsBodies.clear(); +} + void PhysicsServerCommandProcessor::deleteDynamicsWorld() { + deleteCachedInverseDynamicsBodies(); + for (int i=0;im_multiBodyJointFeedbacks.size();i++) { @@ -1869,9 +1889,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm case CMD_RESET_SIMULATION: { //clean up all data - - - + deleteCachedInverseDynamicsBodies(); + if (m_data && m_data->m_guiHelper) { m_data->m_guiHelper->removeAllGraphicsInstances(); @@ -2066,6 +2085,74 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_CALCULATE_INVERSE_DYNAMICS: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + btInverseDynamics::MultiBodyTree** treePtrPtr = + m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody); + btInverseDynamics::MultiBodyTree* tree = 0; + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + + if (treePtrPtr) + { + tree = *treePtrPtr; + } + else + { + btInverseDynamics::btMultiBodyTreeCreator id_creator; + if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false)) + { + b3Error("error creating tree\n"); + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + else + { + tree = btInverseDynamics::CreateMultiBodyTree(id_creator); + m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree); + } + } + + if (tree) + { + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); + btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs); + for (int i = 0; i < num_dofs; i++) + { + q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i]; + qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; + nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; + } + + if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; + for (int i = 0; i < num_dofs; i++) + { + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i+baseDofs]; + } + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED; + } + else + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + } + + } + else + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + + hasStatus = true; + break; + } + case CMD_APPLY_EXTERNAL_FORCE: { if (m_data->m_verboseOutput) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 7cbc87ab4..692bc2dd9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -29,6 +29,7 @@ protected: bool supportsJointMotor(class btMultiBody* body, int linkIndex); int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes); + void deleteCachedInverseDynamicsBodies(); public: PhysicsServerCommandProcessor(); @@ -38,6 +39,7 @@ public: virtual void createEmptyDynamicsWorld(); virtual void deleteDynamicsWorld(); + virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 64ae61991..ac5781639 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -348,6 +348,23 @@ enum EnumSdfRequestInfoFlags //SDF_REQUEST_INFO_CAMERA=2, }; + +struct CalculateInverseDynamicsArgs +{ + int m_bodyUniqueId; + + double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM]; + double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM]; + double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM]; +}; + +struct CalculateInverseDynamicsResultArgs +{ + int m_bodyUniqueId; + int m_dofCount; + double m_jointForces[MAX_DEGREE_OF_FREEDOM]; +}; + struct SharedMemoryCommand { int m_type; @@ -374,6 +391,7 @@ struct SharedMemoryCommand struct RequestPixelDataArgs m_requestPixelDataArguments; struct PickBodyArgs m_pickBodyArguments; struct ExternalForceArgs m_externalForceArguments; + struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments; }; }; @@ -397,6 +415,7 @@ struct SharedMemoryStatus struct SendDebugLinesArgs m_sendDebugLinesArgs; struct SendPixelDataArgs m_sendPixelDataArguments; struct RigidBodyCreateArgs m_rigidBodyCreateArgs; + struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 10d87d008..0b6542806 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -27,6 +27,7 @@ enum EnumSharedMemoryClientCommand CMD_REMOVE_PICKING_CONSTRAINT_BODY, CMD_REQUEST_CAMERA_IMAGE_DATA, CMD_APPLY_EXTERNAL_FORCE, + CMD_CALCULATE_INVERSE_DYNAMICS, CMD_MAX_CLIENT_COMMANDS }; @@ -59,6 +60,8 @@ enum EnumSharedMemoryServerStatus CMD_BODY_INFO_COMPLETED, CMD_BODY_INFO_FAILED, CMD_INVALID_STATUS, + CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED, + CMD_CALCULATED_INVERSE_DYNAMICS_FAILED, CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index c2ceb1a05..ba761dd82 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -10,7 +10,7 @@ end includedirs {".","../../src", "../ThirdPartyLibs",} links { - "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath" + "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath" } language "C++" @@ -137,7 +137,7 @@ defines {"B3_USE_STANDALONE_EXAMPLE"} includedirs {"../../src"} links { - "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common" + "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common" } initOpenGL() initGlew() @@ -211,7 +211,7 @@ if os.is("Windows") then } links { - "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api" + "BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api" } diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 5540d10de..295499858 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -8,61 +8,60 @@ INCLUDE_DIRECTORIES( SET(pybullet_SRCS pybullet.c - ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp - ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp - ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h - ../../examples/OpenGLWindow/SimpleCamera.cpp - ../../examples/OpenGLWindow/SimpleCamera.h - ../../examples/TinyRenderer/geometry.cpp - ../../examples/TinyRenderer/model.cpp - ../../examples/TinyRenderer/tgaimage.cpp - ../../examples/TinyRenderer/our_gl.cpp - ../../examples/TinyRenderer/TinyRenderer.cpp - ../../examples/SharedMemory/InProcessMemory.cpp - ../../examples/SharedMemory/PhysicsClient.cpp - ../../examples/SharedMemory/PhysicsClient.h - ../../examples/SharedMemory/PhysicsServer.cpp - ../../examples/SharedMemory/PhysicsServer.h - ../../examples/SharedMemory/PhysicsServerExample.cpp - ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp - ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp - ../../examples/SharedMemory/PhysicsServerSharedMemory.h - ../../examples/SharedMemory/PhysicsDirect.cpp - ../../examples/SharedMemory/PhysicsDirect.h - ../../examples/SharedMemory/PhysicsDirectC_API.cpp - ../../examples/SharedMemory/PhysicsDirectC_API.h - ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp - ../../examples/SharedMemory/PhysicsServerCommandProcessor.h - ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp - ../../examples/SharedMemory/PhysicsClientSharedMemory.h - ../../examples/SharedMemory/PhysicsClientC_API.cpp - ../../examples/SharedMemory/PhysicsClientC_API.h - ../../examples/SharedMemory/Win32SharedMemory.cpp - ../../examples/SharedMemory/Win32SharedMemory.h - ../../examples/SharedMemory/PosixSharedMemory.cpp - ../../examples/SharedMemory/PosixSharedMemory.h - ../../examples/Utils/b3ResourcePath.cpp - ../../examples/Utils/b3ResourcePath.h - ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp - ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp - ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp - ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp - ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp - ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h - ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp - ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp - ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp - ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp - ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp - ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp - ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp - ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp - ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp - ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp - ../../examples/MultiThreading/b3PosixThreadSupport.cpp - ../../examples/MultiThreading/b3Win32ThreadSupport.cpp - ../../examples/MultiThreading/b3ThreadSupportInterface.cpp - + ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h + ../../examples/OpenGLWindow/SimpleCamera.cpp + ../../examples/OpenGLWindow/SimpleCamera.h + ../../examples/TinyRenderer/geometry.cpp + ../../examples/TinyRenderer/model.cpp + ../../examples/TinyRenderer/tgaimage.cpp + ../../examples/TinyRenderer/our_gl.cpp + ../../examples/TinyRenderer/TinyRenderer.cpp + ../../examples/SharedMemory/InProcessMemory.cpp + ../../examples/SharedMemory/PhysicsClient.cpp + ../../examples/SharedMemory/PhysicsClient.h + ../../examples/SharedMemory/PhysicsServer.cpp + ../../examples/SharedMemory/PhysicsServer.h + ../../examples/SharedMemory/PhysicsServerExample.cpp + ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp + ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp + ../../examples/SharedMemory/PhysicsServerSharedMemory.h + ../../examples/SharedMemory/PhysicsDirect.cpp + ../../examples/SharedMemory/PhysicsDirect.h + ../../examples/SharedMemory/PhysicsDirectC_API.cpp + ../../examples/SharedMemory/PhysicsDirectC_API.h + ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp + ../../examples/SharedMemory/PhysicsServerCommandProcessor.h + ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory.h + ../../examples/SharedMemory/PhysicsClientC_API.cpp + ../../examples/SharedMemory/PhysicsClientC_API.h + ../../examples/SharedMemory/Win32SharedMemory.cpp + ../../examples/SharedMemory/Win32SharedMemory.h + ../../examples/SharedMemory/PosixSharedMemory.cpp + ../../examples/SharedMemory/PosixSharedMemory.h + ../../examples/Utils/b3ResourcePath.cpp + ../../examples/Utils/b3ResourcePath.h + ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp + ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp + ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp + ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp + ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp + ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp + ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp + ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp + ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/MultiThreading/b3PosixThreadSupport.cpp + ../../examples/MultiThreading/b3Win32ThreadSupport.cpp + ../../examples/MultiThreading/b3ThreadSupportInterface.cpp ) IF(WIN32) diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 39092b889..a0585a128 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -1,7 +1,6 @@ project ("pybullet") - language "C++" kind "SharedLib" targetsuffix ("") @@ -12,7 +11,7 @@ project ("pybullet") defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} hasCL = findOpenCL("clew") - links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} + links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} initOpenGL() initGlew() @@ -20,10 +19,8 @@ project ("pybullet") ".", "../../src", "../ThirdPartyLibs", - "/usr/include/python2.7", } - if os.is("MacOSX") then links{"Cocoa.framework","Python"} end @@ -40,8 +37,69 @@ project ("pybullet") files { "pybullet.c", - "../../examples/ExampleBrowser/ExampleEntries.cpp", + "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", + "../../examples/OpenGLWindow/SimpleCamera.cpp", + "../../examples/OpenGLWindow/SimpleCamera.h", + "../../examples/TinyRenderer/geometry.cpp", + "../../examples/TinyRenderer/model.cpp", + "../../examples/TinyRenderer/tgaimage.cpp", + "../../examples/TinyRenderer/our_gl.cpp", + "../../examples/TinyRenderer/TinyRenderer.cpp", + "../../examples/SharedMemory/InProcessMemory.cpp", + "../../examples/SharedMemory/PhysicsClient.cpp", + "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsServer.cpp", + "../../examples/SharedMemory/PhysicsServer.h", + "../../examples/SharedMemory/PhysicsServerExample.cpp", + "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.h", + "../../examples/SharedMemory/PhysicsDirect.cpp", + "../../examples/SharedMemory/PhysicsDirect.h", + "../../examples/SharedMemory/PhysicsDirectC_API.cpp", + "../../examples/SharedMemory/PhysicsDirectC_API.h", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.h", + "../../examples/SharedMemory/PhysicsClientC_API.cpp", + "../../examples/SharedMemory/PhysicsClientC_API.h", + "../../examples/SharedMemory/Win32SharedMemory.cpp", + "../../examples/SharedMemory/Win32SharedMemory.h", + "../../examples/SharedMemory/PosixSharedMemory.cpp", + "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/Utils/b3ResourcePath.cpp", + "../../examples/Utils/b3ResourcePath.h", + "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", + "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", + "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", + "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", + "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", + "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", + "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", + "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/MultiThreading/b3PosixThreadSupport.cpp", + "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", + "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", } + + includedirs { + _OPTIONS["python_include_dir"], + } + libdirs { + _OPTIONS["python_lib_dir"] + } + if os.is("Linux") then initX11() end diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e00c5f0af..511767ccc 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1163,7 +1163,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &roll, &upAxisIndex, &nearVal, &farVal, &fov)) { - b3RequestCameraImageSetPixelResolution(command,width,height); + b3RequestCameraImageSetPixelResolution(command,width,height); if (pybullet_internalSetVector(objTargetPos, targetPos)) { //printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); @@ -1521,6 +1521,123 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) return Py_None; } +///Given an object id, joint positions, joint velocities and joint accelerations, +///compute the joint forces using Inverse Dynamics +static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args) +{ + int size; + if (0 == sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + size = PySequence_Size(args); + if (size==4) + { + + int bodyIndex; + PyObject* objPositionsQ; + PyObject* objVelocitiesQdot; + PyObject* objAccelerations; + + if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, &objVelocitiesQdot, &objAccelerations)) + { + int szObPos = PySequence_Size(objPositionsQ); + int szObVel = PySequence_Size(objVelocitiesQdot); + int szObAcc = PySequence_Size(objAccelerations); + int numJoints = b3GetNumJoints(sm, bodyIndex); + if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && (szObAcc == numJoints)) + { + int szInBytes = sizeof(double)*numJoints; + int i; + PyObject* pylist = 0; + double* jointPositionsQ = (double*)malloc(szInBytes); + double* jointVelocitiesQdot = (double*)malloc(szInBytes); + double* jointAccelerations = (double*)malloc(szInBytes); + double* jointForcesOutput = (double*)malloc(szInBytes); + + for (i = 0; i < numJoints; i++) + { + jointPositionsQ[i] = pybullet_internalGetFloatFromSequence(objPositionsQ, i); + jointVelocitiesQdot[i] = pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); + jointAccelerations[i] = pybullet_internalGetFloatFromSequence(objAccelerations, i); + } + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(sm, + bodyIndex, jointPositionsQ, jointVelocitiesQdot, jointAccelerations); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) + { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(statusHandle, + &bodyUniqueId, + &dofCount, + 0); + + if (dofCount) + { + b3GetStatusInverseDynamicsJointForces(statusHandle, + 0, + 0, + jointForcesOutput); + { + { + + int i; + pylist = PyTuple_New(dofCount); + for (i = 0; i