Allow to compile pybullet on Windows, using CMake
(got it to run, rename pybullet.dll into pybullet.pyd and copy in c:\python34\dlls) Update test.py Allow to compile pybullet using Python 3.x and 2.7
This commit is contained in:
@@ -1,5 +1,8 @@
|
||||
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
|
||||
#include "../SharedMemory/PhysicsClientC_API.h"
|
||||
#include "../SharedMemory/PhysicsDirectC_API.h"
|
||||
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
|
||||
|
||||
|
||||
#ifdef __APPLE__
|
||||
#include <Python/Python.h>
|
||||
@@ -7,6 +10,13 @@
|
||||
#include <Python.h>
|
||||
#endif
|
||||
|
||||
enum eCONNECT_METHOD
|
||||
{
|
||||
eCONNECT_GUI=1,
|
||||
eCONNECT_DIRECT=2,
|
||||
eCONNECT_SHARED_MEMORY=3,
|
||||
};
|
||||
|
||||
static PyObject *SpamError;
|
||||
static b3PhysicsClientHandle sm=0;
|
||||
|
||||
@@ -20,18 +30,19 @@ pybullet_stepSimulation(PyObject *self, PyObject *args)
|
||||
return NULL;
|
||||
}
|
||||
|
||||
{
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
if (b3CanSubmitCommand(sm))
|
||||
{
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
}
|
||||
}
|
||||
|
||||
if (b3CanSubmitCommand(sm))
|
||||
{
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
}
|
||||
}
|
||||
|
||||
return PyLong_FromLong(1);
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyObject *
|
||||
@@ -44,10 +55,51 @@ pybullet_connectPhysicsServer(PyObject *self, PyObject *args)
|
||||
}
|
||||
|
||||
{
|
||||
sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
|
||||
int method=eCONNECT_GUI;
|
||||
if (!PyArg_ParseTuple(args, "i", &method))
|
||||
{
|
||||
PyErr_SetString(SpamError, "connectPhysicsServer expected argument eCONNECT_GUI, eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
switch (method)
|
||||
{
|
||||
case eCONNECT_GUI:
|
||||
{
|
||||
int argc=0;
|
||||
char* argv[1]={0};
|
||||
|
||||
#ifdef __APPLE__
|
||||
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
|
||||
#else
|
||||
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
case eCONNECT_DIRECT:
|
||||
{
|
||||
sm = b3ConnectPhysicsDirect();
|
||||
break;
|
||||
}
|
||||
case eCONNECT_SHARED_MEMORY:
|
||||
{
|
||||
sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
default:
|
||||
{
|
||||
PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument");
|
||||
return NULL;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
return PyLong_FromLong(1);
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyObject *
|
||||
@@ -63,18 +115,15 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args)
|
||||
sm = 0;
|
||||
}
|
||||
|
||||
return PyLong_FromLong(1);
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
|
||||
static PyObject *
|
||||
pybullet_loadURDF(PyObject* self, PyObject* args)
|
||||
{
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int size= PySequence_Size(args);
|
||||
|
||||
int bodyIndex = -1;
|
||||
@@ -87,6 +136,11 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
|
||||
float startOrnZ = 0;
|
||||
float startOwnW = 1;
|
||||
printf("size=%d\n", size);
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
if (size==1)
|
||||
{
|
||||
if (!PyArg_ParseTuple(args, "s", &urdfFileName))
|
||||
@@ -106,11 +160,11 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
|
||||
return NULL;
|
||||
}
|
||||
{
|
||||
printf("urdf filename = %s\n", urdfFileName);
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
|
||||
|
||||
printf("urdf filename = %s\n", urdfFileName);
|
||||
//setting the initial position, orientation and other arguments are optional
|
||||
b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
@@ -136,43 +190,157 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
|
||||
{
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
|
||||
// ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED);
|
||||
}
|
||||
return PyLong_FromLong(1);
|
||||
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyObject *
|
||||
pybullet_setGravity(PyObject* self, PyObject* args)
|
||||
{
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
{
|
||||
float gravX=0;
|
||||
float gravY=0;
|
||||
float gravZ=-10;
|
||||
int ret;
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
|
||||
if (!PyArg_ParseTuple(args, "fff", &gravX,&gravY,&gravZ))
|
||||
{
|
||||
PyErr_SetString(SpamError, "setGravity expected (x,y,z) values.");
|
||||
return NULL;
|
||||
}
|
||||
ret = b3PhysicsParamSetGravity(command, gravX,gravY, gravZ);
|
||||
//ret = b3PhysicsParamSetTimeStep(command, timeStep);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
//ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
|
||||
}
|
||||
|
||||
if (1)
|
||||
{
|
||||
PyObject *pylist;
|
||||
PyObject *item;
|
||||
int i;
|
||||
int num=3;
|
||||
pylist = PyTuple_New(num);
|
||||
for (i = 0; i < num; i++)
|
||||
{
|
||||
item = PyFloat_FromDouble(i);
|
||||
PyTuple_SetItem(pylist, i, item);
|
||||
}
|
||||
return pylist;
|
||||
}
|
||||
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyObject *
|
||||
pybullet_getNumJoints(PyObject* self, PyObject* args)
|
||||
{
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
{
|
||||
int bodyIndex = -1;
|
||||
int numJoints=0;
|
||||
if (!PyArg_ParseTuple(args, "i", &bodyIndex ))
|
||||
{
|
||||
PyErr_SetString(SpamError, "Expected a body index (integer).");
|
||||
return NULL;
|
||||
}
|
||||
numJoints = b3GetNumJoints(sm,bodyIndex);
|
||||
|
||||
#if PY_MAJOR_VERSION >= 3
|
||||
return PyLong_FromLong(numJoints);
|
||||
#else
|
||||
return PyInt_FromLong(numJoints);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static PyMethodDef SpamMethods[] = {
|
||||
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
|
||||
"Create a multibody by loading a URDF file."},
|
||||
|
||||
{"connect", pybullet_connectPhysicsServer, METH_VARARGS,
|
||||
"Connect to an existing physics server (using shared memory by default)."},
|
||||
|
||||
{"disconnect", pybullet_disconnectPhysicsServer, METH_VARARGS,
|
||||
"Disconnect from the physics server."},
|
||||
|
||||
{"resetSimulation", pybullet_resetSimulation, METH_VARARGS,
|
||||
"Reset the simulation: remove all objects and start from an empty world."},
|
||||
|
||||
{"stepSimulation", pybullet_stepSimulation, METH_VARARGS,
|
||||
|
||||
{"stepSimulation", pybullet_stepSimulation, METH_VARARGS,
|
||||
"Step the simulation using forward dynamics."},
|
||||
|
||||
|
||||
{"setGravity", pybullet_setGravity, METH_VARARGS,
|
||||
"Set the gravity acceleration (x,y,z)."},
|
||||
|
||||
{"getNumsetGravity", pybullet_setGravity, METH_VARARGS,
|
||||
"Set the gravity acceleration (x,y,z)."},
|
||||
{
|
||||
"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
|
||||
"Get the number of joints for an object."},
|
||||
|
||||
{NULL, NULL, 0, NULL} /* Sentinel */
|
||||
};
|
||||
|
||||
#if PY_MAJOR_VERSION >= 3
|
||||
static struct PyModuleDef moduledef = {
|
||||
PyModuleDef_HEAD_INIT,
|
||||
"pybullet", /* m_name */
|
||||
"Python bindings for Bullet", /* m_doc */
|
||||
-1, /* m_size */
|
||||
SpamMethods, /* m_methods */
|
||||
NULL, /* m_reload */
|
||||
NULL, /* m_traverse */
|
||||
NULL, /* m_clear */
|
||||
NULL, /* m_free */
|
||||
};
|
||||
#endif
|
||||
|
||||
PyMODINIT_FUNC
|
||||
#if PY_MAJOR_VERSION >= 3
|
||||
PyInit_pybullet(void)
|
||||
#else
|
||||
initpybullet(void)
|
||||
#endif
|
||||
{
|
||||
|
||||
PyObject *m;
|
||||
m = Py_InitModule("pybullet", SpamMethods);
|
||||
#if PY_MAJOR_VERSION >= 3
|
||||
m = PyModule_Create(&moduledef);
|
||||
#else
|
||||
m = Py_InitModule3("pybullet",
|
||||
SpamMethods, "Python bindings for Bullet");
|
||||
#endif
|
||||
|
||||
if (m == NULL)
|
||||
return;
|
||||
return m;
|
||||
|
||||
PyModule_AddIntConstant (m, "SHARED_MEMORY", eCONNECT_SHARED_MEMORY); // user read
|
||||
PyModule_AddIntConstant (m, "DIRECT", eCONNECT_DIRECT); // user read
|
||||
PyModule_AddIntConstant (m, "GUI", eCONNECT_GUI); // user read
|
||||
|
||||
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
|
||||
Py_INCREF(SpamError);
|
||||
PyModule_AddObject(m, "error", SpamError);
|
||||
#if PY_MAJOR_VERSION >= 3
|
||||
return m;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user