From a9b1544a9f3a30f8537933f81f5a3c601a3cc41e Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 9 Aug 2016 18:40:12 -0700 Subject: [PATCH] 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