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.
This commit is contained in:
erwin coumans
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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<dofCount; i++)
|
||||
PyTuple_SetItem(pylist, i, PyFloat_FromDouble(jointForcesOutput[i]));
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError, "Internal error in calculateInverseDynamics");
|
||||
}
|
||||
}
|
||||
free(jointPositionsQ);
|
||||
free(jointVelocitiesQdot);
|
||||
free(jointAccelerations);
|
||||
free(jointForcesOutput);
|
||||
if (pylist)
|
||||
return pylist;
|
||||
}
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError, "calculateInverseDynamics numJoints needs to be positive and [joint positions], [joint velocities], [joint accelerations] need to match the number of joints.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError, "calculateInverseDynamics expects 4 arguments, body index, [joint positions], [joint velocities], [joint accelerations].");
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError, "calculateInverseDynamics expects 4 arguments, body index, [joint positions], [joint velocities], [joint accelerations].");
|
||||
return NULL;
|
||||
}
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
static PyMethodDef SpamMethods[] = {
|
||||
|
||||
@@ -1587,6 +1704,8 @@ static PyMethodDef SpamMethods[] = {
|
||||
{"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS,
|
||||
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF convention"},
|
||||
|
||||
{ "calculateInverseDynamics", pybullet_calculateInverseDynamics, METH_VARARGS,
|
||||
"Given an object id, joint positions, joint velocities and joint accelerations, compute the joint forces using Inverse Dynamics" },
|
||||
//todo(erwincoumans)
|
||||
//saveSnapshot
|
||||
//loadSnapshot
|
||||
|
||||
Reference in New Issue
Block a user