diff --git a/_clang-format b/_clang-format index 69810190f..43a8b1d04 100644 --- a/_clang-format +++ b/_clang-format @@ -83,6 +83,7 @@ SpacesInContainerLiterals: true SpacesInCStyleCastParentheses: false SpacesInParentheses: false SpacesInSquareBrackets: false +AccessModifierOffset: -4 Standard: Auto TabWidth: 4 UseTab: ForContinuationAndIndentation diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c0465f7fb..b5c70f5bd 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3,11 +3,11 @@ #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" #ifdef BT_ENABLE_ENET #include "../SharedMemory/PhysicsClientUDP_C_API.h" -#endif //BT_ENABLE_ENET +#endif //BT_ENABLE_ENET #ifdef BT_ENABLE_CLSOCKET #include "../SharedMemory/PhysicsClientTCP_C_API.h" -#endif //BT_ENABLE_CLSOCKET +#endif //BT_ENABLE_CLSOCKET #ifdef __APPLE__ #include @@ -24,19 +24,17 @@ #define PyString_FromString PyBytes_FromString #endif - - static PyObject* SpamError; #define MAX_PHYSICS_CLIENTS 1024 static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0}; static int sPhysicsClientsGUI[MAX_PHYSICS_CLIENTS] = {0}; -static int sNumPhysicsClients=0; +static int sNumPhysicsClients = 0; b3PhysicsClientHandle getPhysicsClient(int physicsClientId) { b3PhysicsClientHandle sm; - if ((physicsClientId <0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients1[physicsClientId])) + if ((physicsClientId < 0) || (physicsClientId >= MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients1[physicsClientId])) { return 0; } @@ -55,32 +53,34 @@ b3PhysicsClientHandle getPhysicsClient(int physicsClientId) sNumPhysicsClients--; } return 0; - } -static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) +{ double v = 0.0; PyObject* item; - if (PyList_Check(seq)) { + if (PyList_Check(seq)) + { item = PyList_GET_ITEM(seq, index); v = PyFloat_AsDouble(item); } - else { + else + { item = PyTuple_GET_ITEM(seq, index); v = PyFloat_AsDouble(item); } return v; } - // internal function to set a float matrix[16] // used to initialize camera position with // a view and projection matrix in renderImage() // // // Args: // matrix - float[16] which will be set by values from objMat -static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) +{ int i, len; PyObject* seq; @@ -88,8 +88,10 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { if (seq) { len = PySequence_Size(objMat); - if (len == 16) { - for (i = 0; i < len; i++) { + if (len == 16) + { + for (i = 0; i < len; i++) + { matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); @@ -106,7 +108,8 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { // // // Args: // vector - float[3] which will be set by values from objMat -static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { +static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) +{ int i, len; PyObject* seq = 0; if (objVec == NULL) @@ -115,10 +118,11 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { seq = PySequence_Fast(objVec, "expected a sequence"); if (seq) { - len = PySequence_Size(objVec); - if (len == 3) { - for (i = 0; i < len; i++) { + if (len == 3) + { + for (i = 0; i < len; i++) + { vector[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); @@ -130,7 +134,8 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { } // vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { +static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) +{ int i, len; PyObject* seq; if (obVec == NULL) @@ -140,8 +145,10 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { if (seq) { len = PySequence_Size(obVec); - if (len == 3) { - for (i = 0; i < len; i++) { + if (len == 3) + { + for (i = 0; i < len; i++) + { vector[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); @@ -153,7 +160,8 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { } // vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { +static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) +{ int i, len; PyObject* seq; if (obVec == NULL) @@ -161,8 +169,10 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { seq = PySequence_Fast(obVec, "expected a sequence"); len = PySequence_Size(obVec); - if (len == 4) { - for (i = 0; i < len; i++) { + if (len == 4) + { + for (i = 0; i < len; i++) + { vector[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); @@ -172,203 +182,201 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { return 0; } - - // Step through one timestep of the simulation -static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - b3PhysicsClientHandle sm=0; + static char* kwlist[] = {"physicsClientId", NULL}; + b3PhysicsClientHandle sm = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - if (b3CanSubmitCommand(sm)) { - statusHandle = b3SubmitClientCommandAndWaitStatus( - sm, b3InitStepSimulationCommand(sm)); - statusType = b3GetStatusType(statusHandle); - } - } + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; - Py_INCREF(Py_None); - return Py_None; + if (b3CanSubmitCommand(sm)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus( + sm, b3InitStepSimulationCommand(sm)); + statusType = b3GetStatusType(statusHandle); + } + } + + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, PyObject* keywds) { +static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, PyObject* keywds) +{ + int freeIndex = -1; + int method = eCONNECT_GUI; + int i; + b3PhysicsClientHandle sm = 0; - - int freeIndex = -1; - int method = eCONNECT_GUI; - int i; - b3PhysicsClientHandle sm=0; - - if (sNumPhysicsClients>=MAX_PHYSICS_CLIENTS) + if (sNumPhysicsClients >= MAX_PHYSICS_CLIENTS) { - PyErr_SetString(SpamError, - "Exceeding maximum number of physics connections."); - return NULL; + PyErr_SetString(SpamError, + "Exceeding maximum number of physics connections."); + return NULL; } - - { - - int key = SHARED_MEMORY_KEY; - int udpPort = 1234; - int tcpPort = 6667; - - const char* hostName = "localhost"; - - int size = PySequence_Size(args); - if (size == 1) { - if (!PyArg_ParseTuple(args, "i", &method)) { - PyErr_SetString(SpamError, - "connectPhysicsServer expected argument GUI, " - "DIRECT, SHARED_MEMORY, UDP or TCP"); - return NULL; - } - } + int key = SHARED_MEMORY_KEY; + int udpPort = 1234; + int tcpPort = 6667; - //Only one local in-process GUI connection allowed. - if (method == eCONNECT_GUI) - { - int i; - for (i=0;i=0) - { - udpPort = port; - tcpPort = port; - } - } + if (size == 3) + { + int port = -1; + if (!PyArg_ParseTuple(args, "isi", &method, &hostName, &port)) + { + PyErr_SetString(SpamError, + "connectPhysicsServer 3 arguments: method, hostname, port"); + return NULL; + } + if (port >= 0) + { + udpPort = port; + tcpPort = port; + } + } - switch (method) { - case eCONNECT_GUI: { - int argc = 0; - char* argv[1] = {0}; + switch (method) + { + case eCONNECT_GUI: + { + int argc = 0; + char* argv[1] = {0}; #ifdef __APPLE__ - sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); #else - sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); #endif - break; - } - case eCONNECT_DIRECT: { - sm = b3ConnectPhysicsDirect(); - break; - } - case eCONNECT_SHARED_MEMORY: { - sm = b3ConnectSharedMemory(key); - break; - } - case eCONNECT_UDP: - { + break; + } + case eCONNECT_DIRECT: + { + sm = b3ConnectPhysicsDirect(); + break; + } + case eCONNECT_SHARED_MEMORY: + { + sm = b3ConnectSharedMemory(key); + break; + } + case eCONNECT_UDP: + { #ifdef BT_ENABLE_ENET - sm = b3ConnectPhysicsUDP(hostName, udpPort); + sm = b3ConnectPhysicsUDP(hostName, udpPort); #else - PyErr_SetString(SpamError, "UDP is not enabled in this pybullet build"); - return NULL; -#endif //BT_ENABLE_ENET + PyErr_SetString(SpamError, "UDP is not enabled in this pybullet build"); + return NULL; +#endif //BT_ENABLE_ENET - break; - } - case eCONNECT_TCP: - { + break; + } + case eCONNECT_TCP: + { #ifdef BT_ENABLE_CLSOCKET - - sm = b3ConnectPhysicsTCP(hostName, tcpPort); + + sm = b3ConnectPhysicsTCP(hostName, tcpPort); #else - PyErr_SetString(SpamError, "TCP is not enabled in this pybullet build"); - return NULL; -#endif //BT_ENABLE_CLSOCKET - - break; - } - - + PyErr_SetString(SpamError, "TCP is not enabled in this pybullet build"); + return NULL; +#endif //BT_ENABLE_CLSOCKET - default: { - PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument"); - return NULL; - } - }; - } + break; + } - if (sm && b3CanSubmitCommand(sm)) - { - for (i=0;i=0) - { - b3SharedMemoryCommandHandle command; + if (sm && b3CanSubmitCommand(sm)) + { + for (i = 0; i < MAX_PHYSICS_CLIENTS; i++) + { + if (sPhysicsClients1[i] == 0) + { + freeIndex = i; + break; + } + } + + if (freeIndex >= 0) + { + b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int statusType; + sPhysicsClients1[freeIndex] = sm; + sPhysicsClientsGUI[freeIndex] = method; + sNumPhysicsClients++; - sPhysicsClients1[freeIndex] = sm; - sPhysicsClientsGUI[freeIndex] = method; - sNumPhysicsClients++; - - command = b3InitSyncBodyInfoCommand(sm); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + command = b3InitSyncBodyInfoCommand(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); #if 0 if (statusType != CMD_BODY_INFO_COMPLETED) @@ -377,110 +385,105 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P return NULL; } #endif - } - - } - return PyInt_FromLong(freeIndex); + } + } + return PyInt_FromLong(freeIndex); } static PyObject* pybullet_disconnectPhysicsServer(PyObject* self, - PyObject* args, - PyObject *keywds) { - + PyObject* args, + PyObject* keywds) +{ int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - b3DisconnectSharedMemory(sm); - sm = 0; - } + { + b3DisconnectSharedMemory(sm); + sm = 0; + } - sPhysicsClients1[physicsClientId] = 0; - sPhysicsClientsGUI[physicsClientId] = 0; - sNumPhysicsClients--; + sPhysicsClients1[physicsClientId] = 0; + sPhysicsClientsGUI[physicsClientId] = 0; + sNumPhysicsClients--; - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args,PyObject *keywds) { +static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args, PyObject* keywds) +{ const char* worldFileName = ""; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char *kwlist[] = { "worldFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &worldFileName,&physicsClientId)) + static char* kwlist[] = {"worldFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &worldFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - b3SharedMemoryCommandHandle command; + b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int statusType; command = b3SaveWorldCommandInit(sm, worldFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_SAVE_WORLD_COMPLETED) { + if (statusType != CMD_SAVE_WORLD_COMPLETED) + { PyErr_SetString(SpamError, "saveWorld command execution failed."); return NULL; } Py_INCREF(Py_None); return Py_None; - - } + } - PyErr_SetString(SpamError, "Cannot execute saveWorld command."); return NULL; - } #define MAX_SDF_BODIES 512 -static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args,PyObject *keywds) +static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args, PyObject* keywds) { const char* bulletFileName = ""; b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command; - int i,numBodies; + int i, numBodies; int bodyIndicesOut[MAX_SDF_BODIES]; PyObject* pylist = 0; b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bulletFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName,&physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bulletFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - command = b3LoadBulletCommandInit(sm, bulletFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -492,22 +495,23 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args,PyObject *ke } numBodies = - b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); - if (numBodies > MAX_SDF_BODIES) { - PyErr_SetString(SpamError, "loadBullet exceeds body capacity"); - return NULL; - } + b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) + { + PyErr_SetString(SpamError, "loadBullet exceeds body capacity"); + return NULL; + } - pylist = PyTuple_New(numBodies); + pylist = PyTuple_New(numBodies); - if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) { - for (i = 0; i < numBodies; i++) { - PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); - } - } - return pylist; - - + if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) + { + for (i = 0; i < numBodies; i++) + { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; } static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* keywds) @@ -518,20 +522,19 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k b3SharedMemoryCommandHandle command; b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bulletFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName,&physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bulletFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - command = b3SaveBulletCommandInit(sm, bulletFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -544,8 +547,6 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k return Py_None; } - - static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds) { const char* mjcfFileName = ""; @@ -558,19 +559,18 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key int bodyIndicesOut[MAX_SDF_BODIES]; PyObject* pylist = 0; int physicsClientId = 0; - static char *kwlist[] = { "mjcfFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &mjcfFileName,&physicsClientId)) + static char* kwlist[] = {"mjcfFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &mjcfFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - command = b3LoadMJCFCommandInit(sm, mjcfFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -580,24 +580,27 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key return NULL; } - numBodies = - b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); - if (numBodies > MAX_SDF_BODIES) { - PyErr_SetString(SpamError, "SDF exceeds body capacity"); - return NULL; - } + numBodies = + b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) + { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } - pylist = PyTuple_New(numBodies); + pylist = PyTuple_New(numBodies); - if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) { - for (i = 0; i < numBodies; i++) { - PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); - } - } - return pylist; + if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) + { + for (i = 0; i < numBodies; i++) + { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; } -static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds) { double fixedTimeStep = -1; int numSolverIterations = -1; @@ -610,16 +613,16 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "physicsClientId", NULL }; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -634,7 +637,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsParamSetNumSolverIterations(command, numSolverIterations); } - if (collisionFilterMode>=0) + if (collisionFilterMode >= 0) { b3PhysicsParamSetCollisionFilterMode(command, collisionFilterMode); } @@ -648,20 +651,20 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar } if (useSplitImpulse >= 0) { - b3PhysicsParamSetUseSplitImpulse(command,useSplitImpulse); + b3PhysicsParamSetUseSplitImpulse(command, useSplitImpulse); } if (splitImpulsePenetrationThreshold >= 0) { b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold); } - if (contactBreakingThreshold>=0) + if (contactBreakingThreshold >= 0) { - b3PhysicsParamSetContactBreakingThreshold(command,contactBreakingThreshold); + b3PhysicsParamSetContactBreakingThreshold(command, contactBreakingThreshold); } //-1 is disables the maxNumCmdPer1ms feature, allow it - if (maxNumCmdPer1ms>=-1) + if (maxNumCmdPer1ms >= -1) { - b3PhysicsParamSetMaxNumCommandsPer1ms(command,maxNumCmdPer1ms); + b3PhysicsParamSetMaxNumCommandsPer1ms(command, maxNumCmdPer1ms); } //ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); @@ -682,668 +685,701 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar return Py_None; } - // Load a robot from a URDF file (universal robot description format) // function can be called without arguments and will default // to position (0,0,1) with orientation(0,0,0,1) // els(x,y,z) or // loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w) -static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* keywds) { - int physicsClientId = 0; - - static char *kwlist[] = { "fileName", "basePosition", "baseOrientation", "useMaximalCoordinates","useFixedBase","physicsClientId", NULL }; + int physicsClientId = 0; - static char *kwlistBackwardCompatible4[] = { "fileName", "startPosX", "startPosY", "startPosZ", NULL }; - static char *kwlistBackwardCompatible8[] = { "fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY","startOrnZ","startOrnW", NULL }; + static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "physicsClientId", NULL}; + static char* kwlistBackwardCompatible4[] = {"fileName", "startPosX", "startPosY", "startPosZ", NULL}; + static char* kwlistBackwardCompatible8[] = {"fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY", "startOrnZ", "startOrnW", NULL}; - int bodyIndex = -1; - const char* urdfFileName = ""; + int bodyIndex = -1; + const char* urdfFileName = ""; - double startPosX = 0.0; - double startPosY = 0.0; - double startPosZ = 0.0; - double startOrnX = 0.0; - double startOrnY = 0.0; - double startOrnZ = 0.0; - double startOrnW = 1.0; - int useMaximalCoordinates = 0; - int useFixedBase = 0; + double startPosX = 0.0; + double startPosY = 0.0; + double startPosZ = 0.0; + double startOrnX = 0.0; + double startOrnY = 0.0; + double startOrnZ = 0.0; + double startOrnW = 1.0; + int useMaximalCoordinates = 0; + int useFixedBase = 0; - int backwardsCompatibilityArgs = 0; -b3PhysicsClientHandle sm=0; - if (PyArg_ParseTupleAndKeywords(args, keywds, "sddd", kwlistBackwardCompatible4, &urdfFileName, &startPosX, - &startPosY, &startPosZ)) - { - backwardsCompatibilityArgs = 1; - } - else - { - PyErr_Clear(); - } + int backwardsCompatibilityArgs = 0; + b3PhysicsClientHandle sm = 0; + if (PyArg_ParseTupleAndKeywords(args, keywds, "sddd", kwlistBackwardCompatible4, &urdfFileName, &startPosX, + &startPosY, &startPosZ)) + { + backwardsCompatibilityArgs = 1; + } + else + { + PyErr_Clear(); + } + if (PyArg_ParseTupleAndKeywords(args, keywds, "sddddddd", kwlistBackwardCompatible8, &urdfFileName, &startPosX, + &startPosY, &startPosZ, &startOrnX, &startOrnY, &startOrnZ, &startOrnW)) + { + backwardsCompatibilityArgs = 1; + } + else + { + PyErr_Clear(); + } - if (PyArg_ParseTupleAndKeywords(args, keywds, "sddddddd", kwlistBackwardCompatible8,&urdfFileName, &startPosX, - &startPosY, &startPosZ, &startOrnX, &startOrnY,&startOrnZ, &startOrnW)) - { - backwardsCompatibilityArgs = 1; - } - else - { - PyErr_Clear(); - } + if (!backwardsCompatibilityArgs) + { + PyObject* basePosObj = 0; + PyObject* baseOrnObj = 0; + double basePos[3]; + double baseOrn[4]; - - - if (!backwardsCompatibilityArgs) - { - PyObject* basePosObj = 0; - PyObject* baseOrnObj = 0; - double basePos[3]; - double baseOrn[4]; - - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates,&useFixedBase,&physicsClientId)) - { - - return NULL; - } - else - { - if (basePosObj) - { - if (!pybullet_internalSetVectord(basePosObj, basePos)) - { - PyErr_SetString(SpamError, "Cannot convert basePosition."); - return NULL; - } - startPosX = basePos[0]; - startPosY = basePos[1]; - startPosZ = basePos[2]; - - } - if (baseOrnObj) - { - if (!pybullet_internalSetVector4d(baseOrnObj, baseOrn)) - { - PyErr_SetString(SpamError, "Cannot convert baseOrientation."); - return NULL; - } - startOrnX = baseOrn[0]; - startOrnY = baseOrn[1]; - startOrnZ = baseOrn[2]; - startOrnW = baseOrn[3]; - } - } - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &physicsClientId)) + { + return NULL; + } + else + { + if (basePosObj) + { + if (!pybullet_internalSetVectord(basePosObj, basePos)) + { + PyErr_SetString(SpamError, "Cannot convert basePosition."); + return NULL; + } + startPosX = basePos[0]; + startPosY = basePos[1]; + startPosZ = basePos[2]; + } + if (baseOrnObj) + { + if (!pybullet_internalSetVector4d(baseOrnObj, baseOrn)) + { + PyErr_SetString(SpamError, "Cannot convert baseOrientation."); + return NULL; + } + startOrnX = baseOrn[0]; + startOrnY = baseOrn[1]; + startOrnZ = baseOrn[2]; + startOrnW = baseOrn[3]; + } + } + } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - if (strlen(urdfFileName)) { - // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", - // startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = - b3LoadUrdfCommandInit(sm, urdfFileName); - - // setting the initial position, orientation and other arguments are - // optional - b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ); - b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY, - startOrnZ, startOrnW); - if (useMaximalCoordinates) + if (strlen(urdfFileName)) { - b3LoadUrdfCommandSetUseMultiBody(command, 0); - } - if (useFixedBase) - { - b3LoadUrdfCommandSetUseFixedBase(command, 1); - } + // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", + // startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_URDF_LOADING_COMPLETED) { - PyErr_SetString(SpamError, "Cannot load URDF file."); - return NULL; - } - bodyIndex = b3GetStatusBodyIndex(statusHandle); - } else { - PyErr_SetString(SpamError, - "Empty filename, method expects 1, 4 or 8 arguments."); - return NULL; - } - return PyLong_FromLong(bodyIndex); + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = + b3LoadUrdfCommandInit(sm, urdfFileName); + + // setting the initial position, orientation and other arguments are + // optional + b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ); + b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY, + startOrnZ, startOrnW); + if (useMaximalCoordinates) + { + b3LoadUrdfCommandSetUseMultiBody(command, 0); + } + if (useFixedBase) + { + b3LoadUrdfCommandSetUseFixedBase(command, 1); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_URDF_LOADING_COMPLETED) + { + PyErr_SetString(SpamError, "Cannot load URDF file."); + return NULL; + } + bodyIndex = b3GetStatusBodyIndex(statusHandle); + } + else + { + PyErr_SetString(SpamError, + "Empty filename, method expects 1, 4 or 8 arguments."); + return NULL; + } + return PyLong_FromLong(bodyIndex); } +static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keywds) +{ + const char* sdfFileName = ""; + int numBodies = 0; + int i; + int bodyIndicesOut[MAX_SDF_BODIES]; + PyObject* pylist = 0; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle; + b3PhysicsClientHandle sm = 0; - - -static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject *keywds) { - const char* sdfFileName = ""; - int numBodies = 0; - int i; - int bodyIndicesOut[MAX_SDF_BODIES]; - PyObject* pylist = 0; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle; -b3PhysicsClientHandle sm = 0; - - int physicsClientId = 0; - static char *kwlist[] = { "sdfFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &sdfFileName,&physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"sdfFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &sdfFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_SDF_LOADING_COMPLETED) { - PyErr_SetString(SpamError, "Cannot load SDF file."); - return NULL; - } + commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_SDF_LOADING_COMPLETED) + { + PyErr_SetString(SpamError, "Cannot load SDF file."); + return NULL; + } - numBodies = - b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); - if (numBodies > MAX_SDF_BODIES) { - PyErr_SetString(SpamError, "SDF exceeds body capacity"); - return NULL; - } + numBodies = + b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) + { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } - pylist = PyTuple_New(numBodies); + pylist = PyTuple_New(numBodies); - if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) { - for (i = 0; i < numBodies; i++) { - PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); - } - } - return pylist; + if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) + { + for (i = 0; i < numBodies; i++) + { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; } // Reset the simulation to remove all loaded objects -static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds) { - - int physicsClientId = 0; - static char *kwlist[] = { "physicsClientId", NULL }; +static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + static char* kwlist[] = {"physicsClientId", NULL}; b3PhysicsClientHandle sm = 0; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus( - sm, b3InitResetSimulationCommand(sm)); - } - Py_INCREF(Py_None); - return Py_None; + { + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + sm, b3InitResetSimulationCommand(sm)); + } + Py_INCREF(Py_None); + return Py_None; } //this method is obsolete, use pybullet_setJointMotorControl2 instead -static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { - int size; - int bodyIndex, jointIndex, controlMode; +static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) +{ + int size; + int bodyIndex, jointIndex, controlMode; - double targetPosition = 0.0; - double targetVelocity = 0.0; - double maxForce = 100000.0; - double appliedForce = 0.0; - double kp = 0.1; - double kd = 1.0; - int valid = 0; - int physicsClientId = 0; - b3PhysicsClientHandle sm; + double targetPosition = 0.0; + double targetVelocity = 0.0; + double maxForce = 100000.0; + double appliedForce = 0.0; + double kp = 0.1; + double kd = 1.0; + int valid = 0; + int physicsClientId = 0; + b3PhysicsClientHandle sm; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - size = PySequence_Size(args); - if (size == 4) { - double targetValue = 0.0; - // see switch statement below for convertsions dependent on controlMode - if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, - &targetValue)) { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - switch (controlMode) { - case CONTROL_MODE_POSITION_VELOCITY_PD: { - targetPosition = targetValue; - break; - } - case CONTROL_MODE_VELOCITY: { - targetVelocity = targetValue; - break; - } - case CONTROL_MODE_TORQUE: { - appliedForce = targetValue; - break; - } - default: { valid = 0; } - } - } - if (size == 5) { - double targetValue = 0.0; - // See switch statement for conversions - if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, - &targetValue, &maxForce)) { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; + size = PySequence_Size(args); + if (size == 4) + { + double targetValue = 0.0; + // see switch statement below for convertsions dependent on controlMode + if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, + &targetValue)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: + { + appliedForce = targetValue; + break; + } + default: + { + valid = 0; + } + } + } + if (size == 5) + { + double targetValue = 0.0; + // See switch statement for conversions + if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, + &targetValue, &maxForce)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; - switch (controlMode) { - case CONTROL_MODE_POSITION_VELOCITY_PD: { - targetPosition = targetValue; - break; - } - case CONTROL_MODE_VELOCITY: { - targetVelocity = targetValue; - break; - } - case CONTROL_MODE_TORQUE: { - valid = 0; - break; - } - default: { valid = 0; } - } - } - if (size == 6) { - double gain = 0.0; - double targetValue = 0.0; - if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, - &targetValue, &maxForce, &gain)) { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: + { + valid = 0; + break; + } + default: + { + valid = 0; + } + } + } + if (size == 6) + { + double gain = 0.0; + double targetValue = 0.0; + if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, + &targetValue, &maxForce, &gain)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; - switch (controlMode) { - case CONTROL_MODE_POSITION_VELOCITY_PD: { - targetPosition = targetValue; - kp = gain; - break; - } - case CONTROL_MODE_VELOCITY: { - targetVelocity = targetValue; - kd = gain; - break; - } - case CONTROL_MODE_TORQUE: { - valid = 0; - break; - } - default: { valid = 0; } - } - } - if (size == 8) { - // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. - if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, - &controlMode, &targetPosition, &targetVelocity, - &maxForce, &kp, &kd)) { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - } + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + kp = gain; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + kd = gain; + break; + } + case CONTROL_MODE_TORQUE: + { + valid = 0; + break; + } + default: + { + valid = 0; + } + } + } + if (size == 8) + { + // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. + if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, + &controlMode, &targetPosition, &targetVelocity, + &maxForce, &kp, &kd)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + } - if (valid) { - int numJoints; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - struct b3JointInfo info; + if (valid) + { + int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; - numJoints = b3GetNumJoints(sm, bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } - if ((controlMode != CONTROL_MODE_VELOCITY) && - (controlMode != CONTROL_MODE_TORQUE) && - (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); - return NULL; - } + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; + } - commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); + commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - switch (controlMode) { - case CONTROL_MODE_VELOCITY: { - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, - targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); - break; - } + switch (controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + break; + } - case CONTROL_MODE_TORQUE: { - b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, - appliedForce); - break; - } + case CONTROL_MODE_TORQUE: + { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, + appliedForce); + break; + } - case CONTROL_MODE_POSITION_VELOCITY_PD: { - b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, - targetPosition); - b3JointControlSetKp(commandHandle, info.m_uIndex, kp); - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, - targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); - break; - } - default: {} - }; + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, + targetPosition); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + break; + } + default: + { + } + }; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); - return NULL; + Py_INCREF(Py_None); + return Py_None; + } + PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); + return NULL; } - -static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds) { - int bodyIndex, jointIndex, controlMode; + int bodyIndex, jointIndex, controlMode; - double targetPosition = 0.0; - double targetVelocity = 0.0; - double force = 100000.0; - double kp = 0.1; - double kd = 1.0; -b3PhysicsClientHandle sm = 0; + double targetPosition = 0.0; + double targetVelocity = 0.0; + double force = 100000.0; + double kp = 0.1; + double kd = 1.0; + b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyIndex", "jointIndex", "controlMode", "targetPosition", "targetVelocity" - , "force", "positionGain", "velocityGain", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist,&bodyIndex, &jointIndex, &controlMode, - &targetPosition, &targetVelocity,&force, &kp, &kd, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyIndex", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist, &bodyIndex, &jointIndex, &controlMode, + &targetPosition, &targetVelocity, &force, &kp, &kd, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - int numJoints; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - struct b3JointInfo info; + { + int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; - numJoints = b3GetNumJoints(sm, bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } - if ((controlMode != CONTROL_MODE_VELOCITY) && - (controlMode != CONTROL_MODE_TORQUE) && - (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); - return NULL; - } + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; + } - commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); + commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - switch (controlMode) { - case CONTROL_MODE_VELOCITY: { - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, - targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); - break; - } + switch (controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); + break; + } - case CONTROL_MODE_TORQUE: { - b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, - force); - break; - } + case CONTROL_MODE_TORQUE: + { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, + force); + break; + } - case CONTROL_MODE_POSITION_VELOCITY_PD: { - b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, - targetPosition); - b3JointControlSetKp(commandHandle, info.m_uIndex, kp); - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, - targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); - break; - } - default: {} - }; + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, + targetPosition); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); + break; + } + default: + { + } + }; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } -// PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); -// return NULL; + Py_INCREF(Py_None); + return Py_None; + } + // PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); + // return NULL; } static PyObject* pybullet_setRealTimeSimulation(PyObject* self, - PyObject* args, - PyObject* keywds) { - - int enableRealTimeSimulation = 0; - int ret; + PyObject* args, + PyObject* keywds) +{ + int enableRealTimeSimulation = 0; + int ret; b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "enableRealTimeSimulation", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&enableRealTimeSimulation, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"enableRealTimeSimulation", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &enableRealTimeSimulation, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - + { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + ret = + b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); - ret = - b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } - - static PyObject* pybullet_setInternalSimFlags(PyObject* self, - PyObject* args, PyObject* keywds) { - - int flags = 0; - int ret; - int physicsClientId = 0; -b3PhysicsClientHandle sm = 0; + PyObject* args, PyObject* keywds) +{ + int flags = 0; + int ret; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "flags", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&flags, &physicsClientId)) + static char* kwlist[] = {"flags", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &flags, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; - ret = - b3PhysicsParamSetInternalSimFlags(command, flags); + ret = + b3PhysicsParamSetInternalSimFlags(command, flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } // Set the gravity of the world with (x, y, z) arguments -static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* keywds) { - - { - double gravX = 0.0; - double gravY = 0.0; - double gravZ = -10.0; - int ret; - b3PhysicsClientHandle sm = 0; - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - - int physicsClientId = 0; - static char *kwlist[] = { "gravX", "gravY", "gravZ", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ddd|i", kwlist,&gravX, &gravY, &gravZ, &physicsClientId)) +static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* keywds) +{ { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - + double gravX = 0.0; + double gravY = 0.0; + double gravZ = -10.0; + int ret; + b3PhysicsClientHandle sm = 0; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; - command = b3InitPhysicsParamCommand(sm); - - ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ); - // ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } + int physicsClientId = 0; + static char* kwlist[] = {"gravX", "gravY", "gravZ", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ddd|i", kwlist, &gravX, &gravY, &gravZ, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - Py_INCREF(Py_None); - return Py_None; + command = b3InitPhysicsParamCommand(sm); + + ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ); + // ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args, PyObject* keywds) { double timeStep = 0.001; int ret; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "timeStep", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&timeStep, &physicsClientId)) + static char* kwlist[] = {"timeStep", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist, &timeStep, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - + Py_INCREF(Py_None); + return Py_None; + } +} + +static PyObject* +pybullet_setDefaultContactERP(PyObject* self, PyObject* args, PyObject* keywds) +{ + double defaultContactERP = 0.005; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + + static char* kwlist[] = {"defaultContactERP", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist, &defaultContactERP, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + int ret; + + b3SharedMemoryStatusHandle statusHandle; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + ret = b3PhysicsParamSetDefaultContactERP(command, defaultContactERP); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } Py_INCREF(Py_None); return Py_None; } -} - -static PyObject * -pybullet_setDefaultContactERP(PyObject* self, PyObject* args,PyObject* keywds) -{ - double defaultContactERP=0.005; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - - static char *kwlist[] = { "defaultContactERP", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&defaultContactERP, &physicsClientId)) - { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - { - int ret; - - b3SharedMemoryStatusHandle statusHandle; - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - ret = b3PhysicsParamSetDefaultContactERP(command, defaultContactERP); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - } - - Py_INCREF(Py_None); - return Py_None; -} static int pybullet_internalGetBaseVelocity( - int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3],b3PhysicsClientHandle sm) { + int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3], b3PhysicsClientHandle sm) +{ baseLinearVelocity[0] = 0.; baseLinearVelocity[1] = 0.; baseLinearVelocity[2] = 0.; @@ -1352,7 +1388,8 @@ static int pybullet_internalGetBaseVelocity( baseAngularVelocity[1] = 0.; baseAngularVelocity[2] = 0.; - if (0 == sm) { + if (0 == sm) + { PyErr_SetString(SpamError, "Not connected to physics server."); return 0; } @@ -1368,8 +1405,8 @@ static int pybullet_internalGetBaseVelocity( const double* actualStateQdot; // const double* jointReactionForces[]; - - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { PyErr_SetString(SpamError, "getBaseVelocity failed."); return 0; } @@ -1394,7 +1431,6 @@ static int pybullet_internalGetBaseVelocity( baseAngularVelocity[0] = actualStateQdot[3]; baseAngularVelocity[1] = actualStateQdot[4]; baseAngularVelocity[2] = actualStateQdot[5]; - } } return 1; @@ -1403,62 +1439,64 @@ static int pybullet_internalGetBaseVelocity( // Internal function used to get the base position and orientation // Orientation is returned in quaternions static int pybullet_internalGetBasePositionAndOrientation( - int bodyIndex, double basePosition[3], double baseOrientation[4],b3PhysicsClientHandle sm) { - basePosition[0] = 0.; - basePosition[1] = 0.; - basePosition[2] = 0.; + int bodyIndex, double basePosition[3], double baseOrientation[4], b3PhysicsClientHandle sm) +{ + basePosition[0] = 0.; + basePosition[1] = 0.; + basePosition[2] = 0.; - baseOrientation[0] = 0.; - baseOrientation[1] = 0.; - baseOrientation[2] = 0.; - baseOrientation[3] = 1.; + baseOrientation[0] = 0.; + baseOrientation[1] = 0.; + baseOrientation[2] = 0.; + baseOrientation[3] = 1.; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return 0; - } + if (0 == sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return 0; + } - { - { - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + { + { + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - const int status_type = b3GetStatusType(status_handle); - const double* actualStateQ; - // const double* jointReactionForces[]; - + const int status_type = b3GetStatusType(status_handle); + const double* actualStateQ; + // const double* jointReactionForces[]; - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); - return 0; - } + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return 0; + } - b3GetStatusActualState( - status_handle, 0 /* body_unique_id */, - 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, - 0 /*root_local_inertial_frame*/, &actualStateQ, - 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); + b3GetStatusActualState( + status_handle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, &actualStateQ, + 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); - // printf("joint reaction forces="); - // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { - // printf("%f ", jointReactionForces[i]); - // } - // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] - // and orientation x,y,z,w = - // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] - basePosition[0] = actualStateQ[0]; - basePosition[1] = actualStateQ[1]; - basePosition[2] = actualStateQ[2]; + // printf("joint reaction forces="); + // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { + // printf("%f ", jointReactionForces[i]); + // } + // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] + // and orientation x,y,z,w = + // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] + basePosition[0] = actualStateQ[0]; + basePosition[1] = actualStateQ[1]; + basePosition[2] = actualStateQ[2]; - baseOrientation[0] = actualStateQ[3]; - baseOrientation[1] = actualStateQ[4]; - baseOrientation[2] = actualStateQ[5]; - baseOrientation[3] = actualStateQ[6]; - } - } - return 1; + baseOrientation[0] = actualStateQ[3]; + baseOrientation[1] = actualStateQ[4]; + baseOrientation[2] = actualStateQ[5]; + baseOrientation[3] = actualStateQ[6]; + } + } + return 1; } // Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion @@ -1466,92 +1504,97 @@ static int pybullet_internalGetBasePositionAndOrientation( // Object is retrieved based on body index, which is the order // the object was loaded into the simulation (0-based) static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, - PyObject* args, PyObject* keywds) { - int bodyUniqueId = -1; - double basePosition[3]; - double baseOrientation[4]; - PyObject* pylistPos; - PyObject* pylistOrientation; - b3PhysicsClientHandle sm = 0; + PyObject* args, PyObject* keywds) +{ + int bodyUniqueId = -1; + double basePosition[3]; + double baseOrientation[4]; + PyObject* pylistPos; + PyObject* pylistOrientation; + b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - if (0 == pybullet_internalGetBasePositionAndOrientation( - bodyUniqueId, basePosition, baseOrientation,sm)) { - PyErr_SetString(SpamError, - "GetBasePositionAndOrientation failed."); - return NULL; - } - { - PyObject* item; - int i; - int num = 3; - pylistPos = PyTuple_New(num); - for (i = 0; i < num; i++) { - item = PyFloat_FromDouble(basePosition[i]); - PyTuple_SetItem(pylistPos, i, item); - } - } + if (0 == pybullet_internalGetBasePositionAndOrientation( + bodyUniqueId, basePosition, baseOrientation, sm)) + { + PyErr_SetString(SpamError, + "GetBasePositionAndOrientation failed."); + return NULL; + } - { - PyObject* item; - int i; - int num = 4; - pylistOrientation = PyTuple_New(num); - for (i = 0; i < num; i++) { - item = PyFloat_FromDouble(baseOrientation[i]); - PyTuple_SetItem(pylistOrientation, i, item); - } - } + { + PyObject* item; + int i; + int num = 3; + pylistPos = PyTuple_New(num); + for (i = 0; i < num; i++) + { + item = PyFloat_FromDouble(basePosition[i]); + PyTuple_SetItem(pylistPos, i, item); + } + } - { - PyObject* pylist; - pylist = PyTuple_New(2); - PyTuple_SetItem(pylist, 0, pylistPos); - PyTuple_SetItem(pylist, 1, pylistOrientation); - return pylist; - } + { + PyObject* item; + int i; + int num = 4; + pylistOrientation = PyTuple_New(num); + for (i = 0; i < num; i++) + { + item = PyFloat_FromDouble(baseOrientation[i]); + PyTuple_SetItem(pylistOrientation, i, item); + } + } + + { + PyObject* pylist; + pylist = PyTuple_New(2); + PyTuple_SetItem(pylist, 0, pylistPos); + PyTuple_SetItem(pylist, 1, pylistOrientation); + return pylist; + } } - static PyObject* pybullet_getBaseVelocity(PyObject* self, - PyObject* args, PyObject* keywds) { + PyObject* args, PyObject* keywds) +{ int bodyUniqueId = -1; double baseLinearVelocity[3]; double baseAngularVelocity[3]; - PyObject* pylistLinVel=0; - PyObject* pylistAngVel=0; + PyObject* pylistLinVel = 0; + PyObject* pylistAngVel = 0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - + if (0 == pybullet_internalGetBaseVelocity( - bodyUniqueId, baseLinearVelocity, baseAngularVelocity,sm)) { + bodyUniqueId, baseLinearVelocity, baseAngularVelocity, sm)) + { PyErr_SetString(SpamError, - "getBaseVelocity failed."); + "getBaseVelocity failed."); return NULL; } @@ -1560,7 +1603,8 @@ static PyObject* pybullet_getBaseVelocity(PyObject* self, int i; int num = 3; pylistLinVel = PyTuple_New(num); - for (i = 0; i < num; i++) { + for (i = 0; i < num; i++) + { item = PyFloat_FromDouble(baseLinearVelocity[i]); PyTuple_SetItem(pylistLinVel, i, item); } @@ -1571,7 +1615,8 @@ static PyObject* pybullet_getBaseVelocity(PyObject* self, int i; int num = 3; pylistAngVel = PyTuple_New(num); - for (i = 0; i < num; i++) { + for (i = 0; i < num; i++) + { item = PyFloat_FromDouble(baseAngularVelocity[i]); PyTuple_SetItem(pylistAngVel, i, item); } @@ -1590,331 +1635,314 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject* int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - int numBodies = b3GetNumBodies(sm); + { + int numBodies = b3GetNumBodies(sm); #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(numBodies); + return PyLong_FromLong(numBodies); #else - return PyInt_FromLong(numBodies); + return PyInt_FromLong(numBodies); #endif - } + } } - static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds) { -int physicsClientId = 0; -int serialIndex=-1; -b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + int serialIndex = -1; + b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "serialIndex", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&serialIndex, &physicsClientId)) + static char* kwlist[] = {"serialIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &serialIndex, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - - int bodyUniqueId = -1; - bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex); + int bodyUniqueId = -1; + bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex); #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(bodyUniqueId); + return PyLong_FromLong(bodyUniqueId); #else - return PyInt_FromLong(bodyUniqueId); + return PyInt_FromLong(bodyUniqueId); #endif - } + } } static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject* keywds) { - { - int bodyUniqueId= -1; - b3PhysicsClientHandle sm = 0; + int bodyUniqueId = -1; + b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + struct b3BodyInfo info; + if (b3GetBodyInfo(sm, bodyUniqueId, &info)) + { + PyObject* pyListJointInfo = PyTuple_New(1); + PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName)); + return pyListJointInfo; + } + else + { + PyErr_SetString(SpamError, "Couldn't get body info"); + return NULL; + } + } + } + + PyErr_SetString(SpamError, "error in getBodyInfo."); + return NULL; +} + +static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyObject* keywds) +{ + { + int constraintUniqueId = -1; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = {"constraintUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + struct b3UserConstraint constraintInfo; + + if (b3GetUserConstraintInfo(sm, constraintUniqueId, &constraintInfo)) + { + PyObject* pyListConstraintInfo = PyTuple_New(11); + + PyTuple_SetItem(pyListConstraintInfo, 0, PyLong_FromLong(constraintInfo.m_parentBodyIndex)); + PyTuple_SetItem(pyListConstraintInfo, 1, PyLong_FromLong(constraintInfo.m_parentJointIndex)); + PyTuple_SetItem(pyListConstraintInfo, 2, PyLong_FromLong(constraintInfo.m_childBodyIndex)); + PyTuple_SetItem(pyListConstraintInfo, 3, PyLong_FromLong(constraintInfo.m_childJointIndex)); + PyTuple_SetItem(pyListConstraintInfo, 4, PyLong_FromLong(constraintInfo.m_jointType)); + + { + PyObject* axisObj = PyTuple_New(3); + PyTuple_SetItem(axisObj, 0, PyFloat_FromDouble(constraintInfo.m_jointAxis[0])); + PyTuple_SetItem(axisObj, 1, PyFloat_FromDouble(constraintInfo.m_jointAxis[1])); + PyTuple_SetItem(axisObj, 2, PyFloat_FromDouble(constraintInfo.m_jointAxis[2])); + PyTuple_SetItem(pyListConstraintInfo, 5, axisObj); + } + { + PyObject* parentFramePositionObj = PyTuple_New(3); + PyTuple_SetItem(parentFramePositionObj, 0, PyFloat_FromDouble(constraintInfo.m_parentFrame[0])); + PyTuple_SetItem(parentFramePositionObj, 1, PyFloat_FromDouble(constraintInfo.m_parentFrame[1])); + PyTuple_SetItem(parentFramePositionObj, 2, PyFloat_FromDouble(constraintInfo.m_parentFrame[2])); + PyTuple_SetItem(pyListConstraintInfo, 6, parentFramePositionObj); + } + { + PyObject* childFramePositionObj = PyTuple_New(3); + PyTuple_SetItem(childFramePositionObj, 0, PyFloat_FromDouble(constraintInfo.m_childFrame[0])); + PyTuple_SetItem(childFramePositionObj, 1, PyFloat_FromDouble(constraintInfo.m_childFrame[1])); + PyTuple_SetItem(childFramePositionObj, 2, PyFloat_FromDouble(constraintInfo.m_childFrame[2])); + PyTuple_SetItem(pyListConstraintInfo, 7, childFramePositionObj); + } + { + PyObject* parentFrameOrientationObj = PyTuple_New(4); + PyTuple_SetItem(parentFrameOrientationObj, 0, PyFloat_FromDouble(constraintInfo.m_parentFrame[3])); + PyTuple_SetItem(parentFrameOrientationObj, 1, PyFloat_FromDouble(constraintInfo.m_parentFrame[4])); + PyTuple_SetItem(parentFrameOrientationObj, 2, PyFloat_FromDouble(constraintInfo.m_parentFrame[5])); + PyTuple_SetItem(parentFrameOrientationObj, 3, PyFloat_FromDouble(constraintInfo.m_parentFrame[6])); + PyTuple_SetItem(pyListConstraintInfo, 8, parentFrameOrientationObj); + } + { + PyObject* childFrameOrientation = PyTuple_New(4); + PyTuple_SetItem(childFrameOrientation, 0, PyFloat_FromDouble(constraintInfo.m_childFrame[3])); + PyTuple_SetItem(childFrameOrientation, 1, PyFloat_FromDouble(constraintInfo.m_childFrame[4])); + PyTuple_SetItem(childFrameOrientation, 2, PyFloat_FromDouble(constraintInfo.m_childFrame[5])); + PyTuple_SetItem(childFrameOrientation, 3, PyFloat_FromDouble(constraintInfo.m_childFrame[6])); + PyTuple_SetItem(pyListConstraintInfo, 9, childFrameOrientation); + } + PyTuple_SetItem(pyListConstraintInfo, 10, PyFloat_FromDouble(constraintInfo.m_maxAppliedForce)); + + return pyListConstraintInfo; + } + else + { + PyErr_SetString(SpamError, "Couldn't get user constraint info"); + return NULL; + } + } + } + + PyErr_SetString(SpamError, "error in getConstraintInfo."); + return NULL; +} + +static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds) +{ + int numConstraints = 0; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - struct b3BodyInfo info; - if (b3GetBodyInfo(sm,bodyUniqueId,&info)) - { - PyObject* pyListJointInfo = PyTuple_New(1); - PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName)); - return pyListJointInfo; - } else - { - PyErr_SetString(SpamError, "Couldn't get body info"); - return NULL; - } - } - } + numConstraints = b3GetNumUserConstraints(sm); - PyErr_SetString(SpamError, "error in getBodyInfo."); - return NULL; -} - - -static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyObject* keywds) -{ - - { - int constraintUniqueId= -1; - b3PhysicsClientHandle sm = 0; - - int physicsClientId = 0; - static char *kwlist[] = { "constraintUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&constraintUniqueId, &physicsClientId)) - { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - struct b3UserConstraint constraintInfo; - - if (b3GetUserConstraintInfo(sm,constraintUniqueId, &constraintInfo)) - { - PyObject* pyListConstraintInfo = PyTuple_New(11); - - PyTuple_SetItem(pyListConstraintInfo,0,PyLong_FromLong(constraintInfo.m_parentBodyIndex)); - PyTuple_SetItem(pyListConstraintInfo,1,PyLong_FromLong(constraintInfo.m_parentJointIndex)); - PyTuple_SetItem(pyListConstraintInfo,2,PyLong_FromLong(constraintInfo.m_childBodyIndex)); - PyTuple_SetItem(pyListConstraintInfo,3,PyLong_FromLong(constraintInfo.m_childJointIndex)); - PyTuple_SetItem(pyListConstraintInfo,4,PyLong_FromLong(constraintInfo.m_jointType)); - - { - PyObject* axisObj = PyTuple_New(3); - PyTuple_SetItem(axisObj,0,PyFloat_FromDouble(constraintInfo.m_jointAxis[0])); - PyTuple_SetItem(axisObj,1,PyFloat_FromDouble(constraintInfo.m_jointAxis[1])); - PyTuple_SetItem(axisObj,2,PyFloat_FromDouble(constraintInfo.m_jointAxis[2])); - PyTuple_SetItem(pyListConstraintInfo,5,axisObj); - } - { - PyObject* parentFramePositionObj = PyTuple_New(3); - PyTuple_SetItem(parentFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[0])); - PyTuple_SetItem(parentFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[1])); - PyTuple_SetItem(parentFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[2])); - PyTuple_SetItem(pyListConstraintInfo,6,parentFramePositionObj); - } - { - PyObject* childFramePositionObj = PyTuple_New(3); - PyTuple_SetItem(childFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_childFrame[0])); - PyTuple_SetItem(childFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_childFrame[1])); - PyTuple_SetItem(childFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_childFrame[2])); - PyTuple_SetItem(pyListConstraintInfo,7,childFramePositionObj); - } - { - PyObject* parentFrameOrientationObj = PyTuple_New(4); - PyTuple_SetItem(parentFrameOrientationObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[3])); - PyTuple_SetItem(parentFrameOrientationObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[4])); - PyTuple_SetItem(parentFrameOrientationObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[5])); - PyTuple_SetItem(parentFrameOrientationObj,3,PyFloat_FromDouble(constraintInfo.m_parentFrame[6])); - PyTuple_SetItem(pyListConstraintInfo,8,parentFrameOrientationObj); - } - { - PyObject* childFrameOrientation = PyTuple_New(4); - PyTuple_SetItem(childFrameOrientation,0,PyFloat_FromDouble(constraintInfo.m_childFrame[3])); - PyTuple_SetItem(childFrameOrientation,1,PyFloat_FromDouble(constraintInfo.m_childFrame[4])); - PyTuple_SetItem(childFrameOrientation,2,PyFloat_FromDouble(constraintInfo.m_childFrame[5])); - PyTuple_SetItem(childFrameOrientation,3,PyFloat_FromDouble(constraintInfo.m_childFrame[6])); - PyTuple_SetItem(pyListConstraintInfo,9,childFrameOrientation); - } - PyTuple_SetItem(pyListConstraintInfo,10,PyFloat_FromDouble(constraintInfo.m_maxAppliedForce)); - - return pyListConstraintInfo; - } else - { - PyErr_SetString(SpamError, "Couldn't get user constraint info"); - return NULL; - } - } - } - - PyErr_SetString(SpamError, "error in getConstraintInfo."); - return NULL; -} - - -static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds) -{ - int numConstraints = 0; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) - { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - numConstraints = b3GetNumUserConstraints(sm); - #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(numConstraints); + return PyLong_FromLong(numConstraints); #else - return PyInt_FromLong(numConstraints); + return PyInt_FromLong(numConstraints); #endif - - } - - // Return the number of joints in an object based on // body index; body index is based on order of sequence // the object is loaded into simulation -static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args, PyObject* keywds) { int bodyUniqueId = -1; - int numJoints = 0; + int numJoints = 0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - numJoints = b3GetNumJoints(sm, bodyUniqueId); + + numJoints = b3GetNumJoints(sm, bodyUniqueId); #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(numJoints); + return PyLong_FromLong(numJoints); #else - return PyInt_FromLong(numJoints); + return PyInt_FromLong(numJoints); #endif - } // Initalize all joint positions given a list of values -static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObject* keywds) { - +static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObject* keywds) { - int bodyUniqueId; - int jointIndex; - double targetValue; - b3PhysicsClientHandle sm = 0; - - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "jointIndex", "targetValue","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|i", kwlist,&bodyUniqueId, &jointIndex, &targetValue, &physicsClientId)) { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - - { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int numJoints; - - numJoints = b3GetNumJoints(sm, bodyUniqueId); - if ((jointIndex >= numJoints) || (jointIndex < 0)) { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } - - commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); - - b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, - targetValue); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - } - PyErr_SetString(SpamError, "error in resetJointState."); - return NULL; -} - - - - -static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds) -{ - static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity","physicsClientId", NULL }; - - - { - int bodyIndex=0; - PyObject* linVelObj=0; - PyObject* angVelObj=0; - double linVel[3] = { 0, 0, 0 }; - double angVel[3] = { 0, 0, 0 }; - int physicsClientId = 0; + int bodyUniqueId; + int jointIndex; + double targetValue; b3PhysicsClientHandle sm = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist, &bodyIndex, &linVelObj, &angVelObj,&physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "targetValue", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|i", kwlist, &bodyUniqueId, &jointIndex, &targetValue, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int numJoints; + + numJoints = b3GetNumJoints(sm, bodyUniqueId); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); + + b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, + targetValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; + } + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; +} + +static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject* keywds) +{ + static char* kwlist[] = {"objectUniqueId", "linearVelocity", "angularVelocity", "physicsClientId", NULL}; + + { + int bodyIndex = 0; + PyObject* linVelObj = 0; + PyObject* angVelObj = 0; + double linVel[3] = {0, 0, 0}; + double angVel[3] = {0, 0, 0}; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist, &bodyIndex, &linVelObj, &angVelObj, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } if (linVelObj || angVelObj) { - b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -1946,87 +1974,93 @@ static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyOb return NULL; } - - // Reset the position and orientation of the base/root link, position [x,y,z] // and orientation quaternion [x,y,z,w] static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, - PyObject* args, PyObject* keywds) { - - { - int bodyUniqueId; - PyObject* posObj; - PyObject* ornObj; - double pos[3]; - double orn[4]; // as a quaternion - b3PhysicsClientHandle sm = 0; - - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "posObj", "ornObj", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|i", kwlist,&bodyUniqueId, &posObj, &ornObj, &physicsClientId)) + PyObject* args, PyObject* keywds) +{ { - return NULL; + int bodyUniqueId; + PyObject* posObj; + PyObject* ornObj; + double pos[3]; + double orn[4]; // as a quaternion + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "posObj", "ornObj", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|i", kwlist, &bodyUniqueId, &posObj, &ornObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(posObj, "expected a sequence"); + len = PySequence_Size(posObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + pos[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(ornObj, "expected a sequence"); + len = PySequence_Size(ornObj); + if (len == 4) + { + for (i = 0; i < 4; i++) + { + orn[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString( + SpamError, + "orientation needs a 4 coordinates, quaternion [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); + + b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]); + b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], + orn[2], orn[3]); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; + } } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(posObj, "expected a sequence"); - len = PySequence_Size(posObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - pos[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(ornObj, "expected a sequence"); - len = PySequence_Size(ornObj); - if (len == 4) { - for (i = 0; i < 4; i++) { - orn[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString( - SpamError, - "orientation needs a 4 coordinates, quaternion [x,y,z,w]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - - commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); - - b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]); - b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], - orn[2], orn[3]); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - } - PyErr_SetString(SpamError, "error in resetJointState."); - return NULL; + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; } // Get the a single joint info for a specific bodyIndex @@ -2043,67 +2077,70 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, // [int, str, int, int, int, int, float, float] // // TODO(hellojas): get joint positions for a body -static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args,PyObject* keywds) { - PyObject* pyListJointInfo; +static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* pyListJointInfo; - struct b3JointInfo info; + struct b3JointInfo info; - int bodyUniqueId = -1; - int jointIndex = -1; - int jointInfoSize = 8; // size of struct b3JointInfo -b3PhysicsClientHandle sm = 0; -int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "jointIndex", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist,&bodyUniqueId, &jointIndex, &physicsClientId)) + int bodyUniqueId = -1; + int jointIndex = -1; + int jointInfoSize = 8; // size of struct b3JointInfo + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &jointIndex, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { { - // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); + { + // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); - pyListJointInfo = PyTuple_New(jointInfoSize); + pyListJointInfo = PyTuple_New(jointInfoSize); - if (b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info)) { - // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", - // info.m_jointIndex, - // info.m_jointName, - // info.m_jointType, - // info.m_qIndex, - // info.m_uIndex); - // printf(" flags=%d jointDamping=%f jointFriction=%f\n", - // info.m_flags, - // info.m_jointDamping, - // info.m_jointFriction); - PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex)); - PyTuple_SetItem(pyListJointInfo, 1, - PyString_FromString(info.m_jointName)); - PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType)); - PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex)); - PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex)); - PyTuple_SetItem(pyListJointInfo, 5, PyInt_FromLong(info.m_flags)); - PyTuple_SetItem(pyListJointInfo, 6, - PyFloat_FromDouble(info.m_jointDamping)); - PyTuple_SetItem(pyListJointInfo, 7, - PyFloat_FromDouble(info.m_jointFriction)); - return pyListJointInfo; - } else { - PyErr_SetString(SpamError, "GetJointInfo failed."); - return NULL; - } - } - } + if (b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info)) + { + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + return pyListJointInfo; + } + else + { + PyErr_SetString(SpamError, "GetJointInfo failed."); + return NULL; + } + } + } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } // Returns the state of a specific joint in a given bodyIndex @@ -2119,212 +2156,219 @@ int physicsClientId = 0; // TODO(hellojas): check accuracy of position and velocity // TODO(hellojas): check force torque values -static PyObject* pybullet_getJointState(PyObject* self, PyObject* args,PyObject* keywds) { - PyObject* pyListJointForceTorque; - PyObject* pyListJointState; +static PyObject* pybullet_getJointState(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* pyListJointForceTorque; + PyObject* pyListJointState; + struct b3JointSensorState sensorState; - struct b3JointSensorState sensorState; - - int bodyUniqueId = -1; - int jointIndex = -1; - int sensorStateSize = 4; // size of struct b3JointSensorState - int forceTorqueSize = 6; // size of force torque list from b3JointSensorState - int j; + int bodyUniqueId = -1; + int jointIndex = -1; + int sensorStateSize = 4; // size of struct b3JointSensorState + int forceTorqueSize = 6; // size of force torque list from b3JointSensorState + int j; b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "jointIndex","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist,&bodyUniqueId, &jointIndex, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &jointIndex, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - { - int status_type = 0; - b3SharedMemoryCommandHandle cmd_handle; - b3SharedMemoryStatusHandle status_handle; + { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; + if (bodyUniqueId < 0) + { + PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex"); + return NULL; + } + if (jointIndex < 0) + { + PyErr_SetString(SpamError, "getJointState failed; invalid jointIndex"); + return NULL; + } - if (bodyUniqueId < 0) { - PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex"); - return NULL; - } - if (jointIndex < 0) { - PyErr_SetString(SpamError, "getJointState failed; invalid jointIndex"); - return NULL; - } + cmd_handle = + b3RequestActualStateCommandInit(sm, bodyUniqueId); + status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - - cmd_handle = - b3RequestActualStateCommandInit(sm, bodyUniqueId); - status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getJointState failed."); + return NULL; + } - status_type = b3GetStatusType(status_handle); - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - PyErr_SetString(SpamError, "getJointState failed."); - return NULL; - } + pyListJointState = PyTuple_New(sensorStateSize); + pyListJointForceTorque = PyTuple_New(forceTorqueSize); - pyListJointState = PyTuple_New(sensorStateSize); - pyListJointForceTorque = PyTuple_New(forceTorqueSize); + if (b3GetJointState(sm, status_handle, jointIndex, &sensorState)) + { + PyTuple_SetItem(pyListJointState, 0, + PyFloat_FromDouble(sensorState.m_jointPosition)); + PyTuple_SetItem(pyListJointState, 1, + PyFloat_FromDouble(sensorState.m_jointVelocity)); - if (b3GetJointState(sm, status_handle, jointIndex, &sensorState)) - { - PyTuple_SetItem(pyListJointState, 0, - PyFloat_FromDouble(sensorState.m_jointPosition)); - PyTuple_SetItem(pyListJointState, 1, - PyFloat_FromDouble(sensorState.m_jointVelocity)); + for (j = 0; j < forceTorqueSize; j++) + { + PyTuple_SetItem(pyListJointForceTorque, j, + PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); + } - for (j = 0; j < forceTorqueSize; j++) { - PyTuple_SetItem(pyListJointForceTorque, j, - PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); - } + PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); - PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); + PyTuple_SetItem(pyListJointState, 3, + PyFloat_FromDouble(sensorState.m_jointMotorTorque)); - PyTuple_SetItem(pyListJointState, 3, - PyFloat_FromDouble(sensorState.m_jointMotorTorque)); + return pyListJointState; + } + else + { + PyErr_SetString(SpamError, "getJointState failed (2)."); + return NULL; + } + } + } - return pyListJointState; - } else - { - PyErr_SetString(SpamError, "getJointState failed (2)."); - return NULL; - } - } - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args,PyObject* keywds) { - PyObject* pyLinkState; - PyObject* pyLinkStateWorldPosition; - PyObject* pyLinkStateWorldOrientation; - PyObject* pyLinkStateLocalInertialPosition; - PyObject* pyLinkStateLocalInertialOrientation; - PyObject* pyLinkStateWorldLinkFramePosition; - PyObject* pyLinkStateWorldLinkFrameOrientation; +static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* pyLinkState; + PyObject* pyLinkStateWorldPosition; + PyObject* pyLinkStateWorldOrientation; + PyObject* pyLinkStateLocalInertialPosition; + PyObject* pyLinkStateLocalInertialOrientation; + PyObject* pyLinkStateWorldLinkFramePosition; + PyObject* pyLinkStateWorldLinkFrameOrientation; - struct b3LinkState linkState; + struct b3LinkState linkState; - int bodyUniqueId = -1; - int linkIndex = -1; - int i; -b3PhysicsClientHandle sm = 0; + int bodyUniqueId = -1; + int linkIndex = -1; + int i; + b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "linkIndex", "physicsClientId", NULL }; + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { + { - int status_type = 0; - b3SharedMemoryCommandHandle cmd_handle; - b3SharedMemoryStatusHandle status_handle; + { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; - if (bodyUniqueId < 0) { - PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex"); - return NULL; - } - if (linkIndex < 0) { - PyErr_SetString(SpamError, "getLinkState failed; invalid linkIndex"); - return NULL; - } + if (bodyUniqueId < 0) + { + PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex"); + return NULL; + } + if (linkIndex < 0) + { + PyErr_SetString(SpamError, "getLinkState failed; invalid linkIndex"); + return NULL; + } + cmd_handle = + b3RequestActualStateCommandInit(sm, bodyUniqueId); + status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - cmd_handle = - b3RequestActualStateCommandInit(sm, bodyUniqueId); - status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getLinkState failed."); + return NULL; + } - status_type = b3GetStatusType(status_handle); - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - PyErr_SetString(SpamError, "getLinkState failed."); - return NULL; - } + if (b3GetLinkState(sm, status_handle, linkIndex, &linkState)) + { + pyLinkStateWorldPosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateWorldPosition, i, + PyFloat_FromDouble(linkState.m_worldPosition[i])); + } - if (b3GetLinkState(sm, status_handle, linkIndex, &linkState)) - { - pyLinkStateWorldPosition = PyTuple_New(3); - for (i = 0; i < 3; ++i) { - PyTuple_SetItem(pyLinkStateWorldPosition, i, - PyFloat_FromDouble(linkState.m_worldPosition[i])); - } + pyLinkStateWorldOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) + { + PyTuple_SetItem(pyLinkStateWorldOrientation, i, + PyFloat_FromDouble(linkState.m_worldOrientation[i])); + } - pyLinkStateWorldOrientation = PyTuple_New(4); - for (i = 0; i < 4; ++i) { - PyTuple_SetItem(pyLinkStateWorldOrientation, i, - PyFloat_FromDouble(linkState.m_worldOrientation[i])); - } + pyLinkStateLocalInertialPosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateLocalInertialPosition, i, + PyFloat_FromDouble(linkState.m_localInertialPosition[i])); + } - pyLinkStateLocalInertialPosition = PyTuple_New(3); - for (i = 0; i < 3; ++i) { - PyTuple_SetItem(pyLinkStateLocalInertialPosition, i, - PyFloat_FromDouble(linkState.m_localInertialPosition[i])); - } + pyLinkStateLocalInertialOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) + { + PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i, + PyFloat_FromDouble(linkState.m_localInertialOrientation[i])); + } - pyLinkStateLocalInertialOrientation = PyTuple_New(4); - for (i = 0; i < 4; ++i) { - PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i, - PyFloat_FromDouble(linkState.m_localInertialOrientation[i])); - } + pyLinkStateWorldLinkFramePosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateWorldLinkFramePosition, i, + PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i])); + } - pyLinkStateWorldLinkFramePosition = PyTuple_New(3); - for (i = 0; i < 3; ++i) { - PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i, - PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i])); - } + pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) + { + PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i, + PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i])); + } - pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4); - for (i = 0; i < 4; ++i) { - PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i, - PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i])); - } + pyLinkState = PyTuple_New(6); + PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition); + PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation); + PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition); + PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation); + PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition); + PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation); + return pyLinkState; + } + } + } - pyLinkState = PyTuple_New(6); - PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition); - PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation); - PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition); - PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation); - PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition ); - PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation); - - return pyLinkState; - } - } - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } - -static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, PyObject* keywds) { - b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; @@ -2332,28 +2376,28 @@ static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "itemUniqueId", "physicsClientId", NULL }; + static char* kwlist[] = {"itemUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&itemUniqueId, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &itemUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitUserDebugReadParameter(sm,itemUniqueId); + commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) + if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) { - double paramValue=0.f; - int ok = b3GetStatusDebugParameterValue(statusHandle,¶mValue); + double paramValue = 0.f; + int ok = b3GetStatusDebugParameterValue(statusHandle, ¶mValue); if (ok) { PyObject* item = PyFloat_FromDouble(paramValue); @@ -2363,220 +2407,200 @@ static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, PyErr_SetString(SpamError, "Failed to read parameter."); return NULL; - } - - -static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args, PyObject* keywds) { - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; - char* text; + char* text; - - double rangeMin = 0.f; - double rangeMax = 1.f; - double startValue = 0.f; - int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "paramName", "rangeMin", "rangeMax","startValue", "physicsClientId", NULL }; + double rangeMin = 0.f; + double rangeMax = 1.f; + double startValue = 0.f; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"paramName", "rangeMin", "rangeMax", "startValue", "physicsClientId", NULL}; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &text, &rangeMin, &rangeMax,&startValue, &physicsClientId)) - { - return NULL; - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &text, &rangeMin, &rangeMax, &startValue, &physicsClientId)) + { + return NULL; + } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - commandHandle = b3InitUserDebugAddParameter(sm,text,rangeMin,rangeMax, startValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + commandHandle = b3InitUserDebugAddParameter(sm, text, rangeMin, rangeMax, startValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); PyObject* item = PyInt_FromLong(debugItemUniqueId); return item; } - - PyErr_SetString(SpamError, "Error in addUserDebugParameter."); - return NULL; + PyErr_SetString(SpamError, "Error in addUserDebugParameter."); + return NULL; } - -static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject* keywds) { - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int res = 0; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int res = 0; -char* text; - double posXYZ[3]; - double colorRGB[3]={1,1,1}; + char* text; + double posXYZ[3]; + double colorRGB[3] = {1, 1, 1}; - - PyObject* textPositionObj=0; - PyObject* textColorRGBObj=0; - double textSize = 1.f; - double lifeTime = 0.f; - int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime","physicsClientId", NULL }; + PyObject* textPositionObj = 0; + PyObject* textColorRGBObj = 0; + double textSize = 1.f; + double lifeTime = 0.f; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"text", "textPosition", "textColorRGB", "textSize", "lifeTime", "physicsClientId", NULL}; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Oddi", kwlist, &text, &textPositionObj, &textColorRGBObj,&textSize, &lifeTime,&physicsClientId)) - { - return NULL; - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Oddi", kwlist, &text, &textPositionObj, &textColorRGBObj, &textSize, &lifeTime, &physicsClientId)) + { + return NULL; + } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } + res = pybullet_internalSetVectord(textPositionObj, posXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting textPositionObj[3]"); + return NULL; + } - res = pybullet_internalSetVectord(textPositionObj,posXYZ); - if (!res) - { - PyErr_SetString(SpamError, "Error converting textPositionObj[3]"); - return NULL; - } + if (textColorRGBObj) + { + res = pybullet_internalSetVectord(textColorRGBObj, colorRGB); + if (!res) + { + PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]"); + return NULL; + } + } - if (textColorRGBObj) - { - res = pybullet_internalSetVectord(textColorRGBObj,colorRGB); - if (!res) - { - PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]"); - return NULL; - } - } + commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, colorRGB, textSize, lifeTime); - - commandHandle = b3InitUserDebugDrawAddText3D(sm,text,posXYZ,colorRGB,textSize,lifeTime); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + PyObject* item = PyInt_FromLong(debugItemUniqueId); + return item; + } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) - { - int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); - PyObject* item = PyInt_FromLong(debugItemUniqueId); - return item; - } - - PyErr_SetString(SpamError, "Error in addUserDebugText."); - return NULL; + PyErr_SetString(SpamError, "Error in addUserDebugText."); + return NULL; } - -static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject* keywds) { - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int res = 0; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int res = 0; - - double fromXYZ[3]; - double toXYZ[3]; - double colorRGB[3]={1,1,1}; + double fromXYZ[3]; + double toXYZ[3]; + double colorRGB[3] = {1, 1, 1}; - PyObject* lineFromObj=0; - PyObject* lineToObj=0; - PyObject* lineColorRGBObj=0; - double lineWidth = 1.f; - double lifeTime = 0.f; - int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime","physicsClientId", NULL }; + PyObject* lineFromObj = 0; + PyObject* lineToObj = 0; + PyObject* lineColorRGBObj = 0; + double lineWidth = 1.f; + double lifeTime = 0.f; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "physicsClientId", NULL}; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddi", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj,&lineWidth, &lifeTime,&physicsClientId)) - { - return NULL; - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddi", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &physicsClientId)) + { + return NULL; + } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - res = pybullet_internalSetVectord(lineFromObj,fromXYZ); - if (!res) - { - PyErr_SetString(SpamError, "Error converting lineFrom[3]"); - return NULL; - } + res = pybullet_internalSetVectord(lineFromObj, fromXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineFrom[3]"); + return NULL; + } - res = pybullet_internalSetVectord(lineToObj,toXYZ); - if (!res) - { - PyErr_SetString(SpamError, "Error converting lineTo[3]"); - return NULL; - } - if (lineColorRGBObj) - { - res = pybullet_internalSetVectord(lineColorRGBObj,colorRGB); - } - - commandHandle = b3InitUserDebugDrawAddLine3D(sm,fromXYZ,toXYZ,colorRGB,lineWidth,lifeTime); + res = pybullet_internalSetVectord(lineToObj, toXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineTo[3]"); + return NULL; + } + if (lineColorRGBObj) + { + res = pybullet_internalSetVectord(lineColorRGBObj, colorRGB); + } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) - { - int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); - PyObject* item = PyInt_FromLong(debugItemUniqueId); - return item; - } + commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, colorRGB, lineWidth, lifeTime); - PyErr_SetString(SpamError, "Error in addUserDebugLine."); - return NULL; + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + PyObject* item = PyInt_FromLong(debugItemUniqueId); + return item; + } + + PyErr_SetString(SpamError, "Error in addUserDebugLine."); + return NULL; } - - - -static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject* keywds) { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int itemUniqueId; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int itemUniqueId; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "itemUniqueId", "physicsClientId", NULL }; + static char* kwlist[] = {"itemUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&itemUniqueId, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &itemUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId); + commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); @@ -2585,38 +2609,36 @@ static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, Py return Py_None; } -static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject* keywds) { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitUserDebugDrawRemoveAll(sm); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryStatusHandle statusHandle; int statusType; @@ -2625,17 +2647,17 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb int loggingType = -1; char* fileName = 0; PyObject* objectUniqueIdsObj = 0; - int maxLogDof=-1; + int maxLogDof = -1; - static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "maxLogDof", "physicsClientId", NULL }; + static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "physicsClientId", NULL}; int physicsClientId = 0; - + if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oii", kwlist, - &loggingType, &fileName, &objectUniqueIdsObj,&maxLogDof, &physicsClientId)) + &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -2643,8 +2665,8 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb { b3SharedMemoryCommandHandle commandHandle; commandHandle = b3StateLoggingCommandInit(sm); - - b3StateLoggingStart(commandHandle,loggingType,fileName); + + b3StateLoggingStart(commandHandle, loggingType, fileName); if (objectUniqueIdsObj) { @@ -2653,23 +2675,23 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb { int len = PySequence_Size(objectUniqueIdsObj); int i; - for ( i=0;i0) + if (maxLogDof > 0) { b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); } - + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_STATE_LOGGING_START_COMPLETED) + if (statusType == CMD_STATE_LOGGING_START_COMPLETED) { int loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); PyObject* loggingUidObj = PyInt_FromLong(loggingUniqueId); @@ -2681,140 +2703,139 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb return Py_None; } -static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryStatusHandle statusHandle; int statusType; - int loggingId=-1; + int loggingId = -1; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "loggingId", "physicsClientId", NULL }; - int physicsClientId = 0; - + static char* kwlist[] = {"loggingId", "physicsClientId", NULL}; + int physicsClientId = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, - &loggingId,&physicsClientId)) + &loggingId, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - if (loggingId>=0) + if (loggingId >= 0) { b3SharedMemoryCommandHandle commandHandle; commandHandle = b3StateLoggingCommandInit(sm); b3StateLoggingStop(commandHandle, loggingId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - } Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject* keywds) { - static char *kwlist[] = { "timeOutInSeconds", "physicsClientId", NULL }; + static char* kwlist[] = {"timeOutInSeconds", "physicsClientId", NULL}; double timeOutInSeconds = -1; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist, - &timeOutInSeconds, &physicsClientId)) + &timeOutInSeconds, &physicsClientId)) return NULL; - if (timeOutInSeconds>=0) + if (timeOutInSeconds >= 0) { sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - b3SetTimeOut(sm,timeOutInSeconds); + b3SetTimeOut(sm, timeOutInSeconds); } Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; - PyObject* rayFromObj=0; - PyObject* rayToObj=0; + PyObject* rayFromObj = 0; + PyObject* rayToObj = 0; double from[3]; double to[3]; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "rayFromPosition", "rayToPosition", "physicsClientId", NULL }; - int physicsClientId = 0; - + static char* kwlist[] = {"rayFromPosition", "rayToPosition", "physicsClientId", NULL}; + int physicsClientId = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, - &rayFromObj, &rayToObj,&physicsClientId)) + &rayFromObj, &rayToObj, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - pybullet_internalSetVectord(rayFromObj,from); - pybullet_internalSetVectord(rayToObj,to); + pybullet_internalSetVectord(rayFromObj, from); + pybullet_internalSetVectord(rayToObj, to); - commandHandle = b3CreateRaycastCommandInit(sm, from[0],from[1],from[2], - to[0],to[1],to[2]); + commandHandle = b3CreateRaycastCommandInit(sm, from[0], from[1], from[2], + to[0], to[1], to[2]); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) + if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) { struct b3RaycastInformation raycastInfo; PyObject* rayHitsObj = 0; int i; b3GetRaycastInformation(sm, &raycastInfo); - + rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits); - for (i=0;i=-1) + if (trackObjectUid >= -1) { - b3SetVRCameraTrackingObject(commandHandle,trackObjectUid); + b3SetVRCameraTrackingObject(commandHandle, trackObjectUid); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); @@ -2902,10 +2922,9 @@ static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObj Py_INCREF(Py_None); return Py_None; - } -static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -2914,16 +2933,16 @@ static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyOb b3PhysicsClientHandle sm = 0; struct b3KeyboardEventsData keyboardEventsData; PyObject* keyEventsObj = 0; - int i=0; + int i = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } - + sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -2931,166 +2950,161 @@ static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyOb commandHandle = b3RequestKeyboardEventsCommandInit(sm); b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - b3GetKeyboardEventsData(sm,&keyboardEventsData); + b3GetKeyboardEventsData(sm, &keyboardEventsData); keyEventsObj = PyDict_New(); - - for (i=0;i=0) && (cameraYaw>=0) && (cameraPitch>=0)) + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"cameraDistance", "cameraYaw", "cameraPitch", "cameraTargetPosition", "physicsClientId", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "fffO|i", kwlist, + &cameraDistance, &cameraYaw, &cameraPitch, &cameraTargetPosObj, &physicsClientId)) + return NULL; + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(sm); + if ((cameraDistance >= 0) && (cameraYaw >= 0) && (cameraPitch >= 0)) { float cameraTargetPosition[3]; - if (pybullet_internalSetVector(cameraTargetPosObj,cameraTargetPosition)) + if (pybullet_internalSetVector(cameraTargetPosObj, cameraTargetPosition)) { - b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle,cameraDistance,cameraPitch, cameraYaw, cameraTargetPosition); + b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, cameraTargetPosition); } } - b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - } - Py_INCREF(Py_None); - return Py_None; + b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* objectColorRGBObj = 0; double objectColorRGB[3]; @@ -3098,15 +3112,15 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py int objectUniqueId = -1; int linkIndex = -2; int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "objectUniqueId", "linkIndex","objectDebugColorRGB", "physicsClientId", NULL }; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"objectUniqueId", "linkIndex", "objectDebugColorRGB", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|Oi", kwlist, - &objectUniqueId, &linkIndex, &objectColorRGBObj,&physicsClientId)) + &objectUniqueId, &linkIndex, &objectColorRGBObj, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -3130,9 +3144,8 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py Py_INCREF(Py_None); return Py_None; } - -static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds) { int objectUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; @@ -3143,22 +3156,19 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyObject* pyResultList = 0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "objectUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&objectUniqueId,&physicsClientId)) + static char* kwlist[] = {"objectUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &objectUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - - commandHandle = b3InitRequestVisualShapeInformation(sm, objectUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); @@ -3172,10 +3182,10 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyObject* item; item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId); PyTuple_SetItem(visualShapeObList, 0, item); - + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_linkIndex); PyTuple_SetItem(visualShapeObList, 1, item); - + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_visualGeometryType); PyTuple_SetItem(visualShapeObList, 2, item); @@ -3189,7 +3199,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyTuple_SetItem(vec, 2, item); PyTuple_SetItem(visualShapeObList, 3, vec); } - + item = PyString_FromString(visualShapeInfo.m_visualShapeData[i].m_meshAssetFileName); PyTuple_SetItem(visualShapeObList, 4, item); @@ -3217,19 +3227,18 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyTuple_SetItem(visualShapeObList, 6, vec); } - { - PyObject* rgba = PyTuple_New(4); - item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[0]); - PyTuple_SetItem(rgba, 0, item); - item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[1]); - PyTuple_SetItem(rgba, 1, item); - item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[2]); - PyTuple_SetItem(rgba, 2, item); - item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[3]); - PyTuple_SetItem(rgba, 3, item); - PyTuple_SetItem(visualShapeObList, 7, rgba); - } - + { + PyObject* rgba = PyTuple_New(4); + item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[0]); + PyTuple_SetItem(rgba, 0, item); + item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[1]); + PyTuple_SetItem(rgba, 1, item); + item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[2]); + PyTuple_SetItem(rgba, 2, item); + item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[3]); + PyTuple_SetItem(rgba, 3, item); + PyTuple_SetItem(visualShapeObList, 7, rgba); + } PyTuple_SetItem(pyResultList, i, visualShapeObList); } @@ -3246,94 +3255,89 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO return Py_None; } -static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args,PyObject* keywds) +static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds) { - - int objectUniqueId = -1; - int jointIndex = -1; - int shapeIndex = -1; - int textureUniqueId = -1; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int physicsClientId = 0; + int objectUniqueId = -1; + int jointIndex = -1; + int shapeIndex = -1; + int textureUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "objectUniqueId", "jointIndex", "shapeIndex", "textureUniqueId", "physicsClientId", NULL }; + static char* kwlist[] = {"objectUniqueId", "jointIndex", "shapeIndex", "textureUniqueId", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiii|i", kwlist, &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - - commandHandle = b3InitUpdateVisualShape(sm, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED) - { - } - else - { - PyErr_SetString(SpamError, "Error resetting visual shape info"); - return NULL; - } - } - - Py_INCREF(Py_None); - return Py_None; + { + commandHandle = b3InitUpdateVisualShape(sm, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED) + { + } + else + { + PyErr_SetString(SpamError, "Error resetting visual shape info"); + return NULL; + } + } + + Py_INCREF(Py_None); + return Py_None; } static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* keywds) { - const char* filename = 0; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - + const char* filename = 0; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "textureFilename", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist,&filename, &physicsClientId)) + static char* kwlist[] = {"textureFilename", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &filename, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - - commandHandle = b3InitLoadTexture(sm, filename); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_LOAD_TEXTURE_COMPLETED) - { - } - else - { - PyErr_SetString(SpamError, "Error loading texture"); - return NULL; - } - } - - Py_INCREF(Py_None); - return Py_None; + + { + commandHandle = b3InitLoadTexture(sm, filename); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_LOAD_TEXTURE_COMPLETED) + { + } + else + { + PyErr_SetString(SpamError, "Error loading texture"); + return NULL; + } + } + + Py_INCREF(Py_None); + return Py_None; } - -static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPointPtr) +static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPointPtr) { - /* + /* 0 int m_contactFlags; 1 int m_bodyUniqueIdA; 2 int m_bodyUniqueIdB; @@ -3353,88 +3357,87 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin int i; PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints); - for (i = 0; i < contactPointPtr->m_numContactPoints; i++) { - PyObject* contactObList = PyTuple_New(10); // see above 10 fields - PyObject* item; - item = - PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags); - PyTuple_SetItem(contactObList, 0, item); - item = PyInt_FromLong( - contactPointPtr->m_contactPointData[i].m_bodyUniqueIdA); - PyTuple_SetItem(contactObList, 1, item); - item = PyInt_FromLong( - contactPointPtr->m_contactPointData[i].m_bodyUniqueIdB); - PyTuple_SetItem(contactObList, 2, item); - item = - PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexA); - PyTuple_SetItem(contactObList, 3, item); - item = - PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB); - PyTuple_SetItem(contactObList, 4, item); + for (i = 0; i < contactPointPtr->m_numContactPoints; i++) + { + PyObject* contactObList = PyTuple_New(10); // see above 10 fields + PyObject* item; + item = + PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags); + PyTuple_SetItem(contactObList, 0, item); + item = PyInt_FromLong( + contactPointPtr->m_contactPointData[i].m_bodyUniqueIdA); + PyTuple_SetItem(contactObList, 1, item); + item = PyInt_FromLong( + contactPointPtr->m_contactPointData[i].m_bodyUniqueIdB); + PyTuple_SetItem(contactObList, 2, item); + item = + PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexA); + PyTuple_SetItem(contactObList, 3, item); + item = + PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB); + PyTuple_SetItem(contactObList, 4, item); - { - PyObject* posAObj = PyTuple_New(3); + { + PyObject* posAObj = PyTuple_New(3); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]); - PyTuple_SetItem(posAObj, 0, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]); - PyTuple_SetItem(posAObj, 1, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]); - PyTuple_SetItem(posAObj, 2, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]); + PyTuple_SetItem(posAObj, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]); + PyTuple_SetItem(posAObj, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]); + PyTuple_SetItem(posAObj, 2, item); - PyTuple_SetItem(contactObList, 5, posAObj); - } + PyTuple_SetItem(contactObList, 5, posAObj); + } - { - PyObject* posBObj = PyTuple_New(3); + { + PyObject* posBObj = PyTuple_New(3); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]); + PyTuple_SetItem(posBObj, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]); + PyTuple_SetItem(posBObj, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]); + PyTuple_SetItem(posBObj, 2, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]); - PyTuple_SetItem(posBObj, 0, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]); - PyTuple_SetItem(posBObj, 1, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]); - PyTuple_SetItem(posBObj, 2, item); + PyTuple_SetItem(contactObList, 6, posBObj); + } - PyTuple_SetItem(contactObList, 6, posBObj); + { + PyObject* normalOnB = PyTuple_New(3); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]); + PyTuple_SetItem(normalOnB, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]); + PyTuple_SetItem(normalOnB, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]); + PyTuple_SetItem(normalOnB, 2, item); + PyTuple_SetItem(contactObList, 7, normalOnB); + } - } + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactDistance); + PyTuple_SetItem(contactObList, 8, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_normalForce); + PyTuple_SetItem(contactObList, 9, item); - { - PyObject* normalOnB = PyTuple_New(3); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]); - PyTuple_SetItem(normalOnB, 0, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]); - PyTuple_SetItem(normalOnB, 1, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]); - PyTuple_SetItem(normalOnB, 2, item); - PyTuple_SetItem(contactObList, 7, normalOnB); - } - - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactDistance); - PyTuple_SetItem(contactObList, 8, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_normalForce); - PyTuple_SetItem(contactObList, 9, item); - - PyTuple_SetItem(pyResultList, i, contactObList); - } - return pyResultList; + PyTuple_SetItem(pyResultList, i, contactObList); + } + return pyResultList; } -static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, PyObject* keywds) { - PyObject* aabbMinOb=0, *aabbMaxOb=0; + PyObject *aabbMinOb = 0, *aabbMaxOb = 0; double aabbMin[3]; double aabbMax[3]; b3SharedMemoryCommandHandle commandHandle; @@ -3443,21 +3446,19 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, int i; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "aabbMin", "aabbMax", "physicsClientId",NULL }; - + static char* kwlist[] = {"aabbMin", "aabbMax", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, - &aabbMinOb, &aabbMaxOb,&physicsClientId)) + &aabbMinOb, &aabbMaxOb, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - pybullet_internalSetVectord(aabbMinOb, aabbMin); pybullet_internalSetVectord(aabbMaxOb, aabbMax); @@ -3471,8 +3472,9 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, //For huge amount of overlap, we could use numpy instead (see camera pixel data) //What would Python do with huge amount of data? Pass it onto TensorFlow! - for (i = 0; i < overlapData.m_numOverlappingObjects; i++) { - PyObject* overlap = PyTuple_New(2);//body unique id and link index + for (i = 0; i < overlapData.m_numOverlappingObjects; i++) + { + PyObject* overlap = PyTuple_New(2); //body unique id and link index PyObject* item; item = @@ -3491,163 +3493,155 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, return Py_None; } - -static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, PyObject* keywds) { - int bodyUniqueIdA = -1; - int bodyUniqueIdB = -1; - int linkIndexA = -2; - int linkIndexB = -2; + int bodyUniqueIdA = -1; + int bodyUniqueIdB = -1; + int linkIndexA = -2; + int linkIndexB = -2; - double distanceThreshold = 0.f; + double distanceThreshold = 0.f; - b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB","physicsClientId", NULL }; + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "physicsClientId", NULL}; - - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iii", kwlist, - &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB,&physicsClientId)) - return NULL; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iii", kwlist, + &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, &physicsClientId)) + return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitClosestDistanceQuery(sm); - b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); - b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB); - b3SetClosestDistanceThreshold(commandHandle, distanceThreshold); - if (linkIndexA >= -1) - { - b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA); - } - if (linkIndexB >= -1) - { - b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB); - } + commandHandle = b3InitClosestDistanceQuery(sm); + b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); + b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB); + b3SetClosestDistanceThreshold(commandHandle, distanceThreshold); + if (linkIndexA >= -1) + { + b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA); + } + if (linkIndexB >= -1) + { + b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB); + } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { - + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + b3GetContactPointInformation(sm, &contactPointData); - b3GetContactPointInformation(sm, &contactPointData); + return MyConvertContactPoint(&contactPointData); + } - return MyConvertContactPoint(&contactPointData); - - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } - -static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { - static char *kwlist[] = { "userConstraintUniqueId","jointChildPivot", "jointChildFrameOrientation","maxForce", "physicsClientId", NULL}; - int userConstraintUniqueId=-1; + static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "physicsClientId", NULL}; + int userConstraintUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - PyObject* jointChildPivotObj=0; - PyObject* jointChildFrameOrnObj=0; + PyObject* jointChildPivotObj = 0; + PyObject* jointChildFrameOrnObj = 0; double jointChildPivot[3]; double jointChildFrameOrn[4]; double maxForce = -1; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOdi", kwlist,&userConstraintUniqueId,&jointChildPivotObj, &jointChildFrameOrnObj,&maxForce, &physicsClientId)) - { - return NULL; - } - - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - commandHandle = b3InitChangeUserConstraintCommand(sm,userConstraintUniqueId); - - if (pybullet_internalSetVectord(jointChildPivotObj,jointChildPivot)) - { - b3InitChangeUserConstraintSetPivotInB(commandHandle, jointChildPivot); - } - if (pybullet_internalSetVector4d(jointChildFrameOrnObj,jointChildFrameOrn)) - { - b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn); - } - if (maxForce>=0) - { - b3InitChangeUserConstraintSetMaxForce(commandHandle,maxForce); - } - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - Py_INCREF(Py_None); - return Py_None; -}; - - -static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) -{ - static char *kwlist[] = { "userConstraintUniqueId","physicsClientId", NULL}; - int userConstraintUniqueId=-1; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&userConstraintUniqueId,&physicsClientId)) - { - return NULL; - } - - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - Py_INCREF(Py_None); - return Py_None; -}; - - -/* -static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) -{ - return NULL; -} -*/ - -static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* args, PyObject *keywds) -{ - int bodyUniqueId = -1; - int jointIndex = -1; - int enableSensor = 1; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - int numJoints = -1; - - static char *kwlist[] = {"bodyUniqueId", "jointIndex" ,"enableSensor", "physicsClientId" }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &jointIndex, &enableSensor,&physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOdi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitChangeUserConstraintCommand(sm, userConstraintUniqueId); + + if (pybullet_internalSetVectord(jointChildPivotObj, jointChildPivot)) + { + b3InitChangeUserConstraintSetPivotInB(commandHandle, jointChildPivot); + } + if (pybullet_internalSetVector4d(jointChildFrameOrnObj, jointChildFrameOrn)) + { + b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn); + } + if (maxForce >= 0) + { + b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + Py_INCREF(Py_None); + return Py_None; +}; + +static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) +{ + static char* kwlist[] = {"userConstraintUniqueId", "physicsClientId", NULL}; + int userConstraintUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userConstraintUniqueId, &physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitRemoveUserConstraintCommand(sm, userConstraintUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + Py_INCREF(Py_None); + return Py_None; +}; + +/* +static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +{ + return NULL; +} +*/ + +static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueId = -1; + int jointIndex = -1; + int enableSensor = 1; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + int numJoints = -1; + + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "enableSensor", "physicsClientId"}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &jointIndex, &enableSensor, &physicsClientId)) { return NULL; } @@ -3659,7 +3653,6 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* return NULL; } - if (bodyUniqueId < 0) { PyErr_SetString(SpamError, "Error: invalid bodyUniqueId"); @@ -3692,71 +3685,66 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* return NULL; } - -static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { - b3SharedMemoryCommandHandle commandHandle; - int parentBodyUniqueId=-1; - int parentLinkIndex=-1; - int childBodyUniqueId=-1; - int childLinkIndex=-1; - int jointType=ePoint2PointType; - PyObject* jointAxisObj=0; - double jointAxis[3]={0,0,0}; + int parentBodyUniqueId = -1; + int parentLinkIndex = -1; + int childBodyUniqueId = -1; + int childLinkIndex = -1; + int jointType = ePoint2PointType; + PyObject* jointAxisObj = 0; + double jointAxis[3] = {0, 0, 0}; PyObject* parentFramePositionObj = 0; - double parentFramePosition[3]={0,0,0}; + double parentFramePosition[3] = {0, 0, 0}; PyObject* childFramePositionObj = 0; - double childFramePosition[3]={0,0,0}; + double childFramePosition[3] = {0, 0, 0}; PyObject* parentFrameOrientationObj = 0; - double parentFrameOrientation[4]={0,0,0,1}; + double parentFrameOrientation[4] = {0, 0, 0, 1}; PyObject* childFrameOrientationObj = 0; - double childFrameOrientation[4]={0,0,0,1}; + double childFrameOrientation[4] = {0, 0, 0, 1}; struct b3JointInfo jointInfo; b3SharedMemoryStatusHandle statusHandle; int statusType; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "parentBodyUniqueId", "parentLinkIndex", - "childBodyUniqueId", "childLinkIndex", - "jointType", - "jointAxis", - "parentFramePosition", - "childFramePosition", - "parentFrameOrientation", - "childFrameOrientation", - "physicsClientId", - NULL }; + static char* kwlist[] = {"parentBodyUniqueId", "parentLinkIndex", + "childBodyUniqueId", "childLinkIndex", + "jointType", + "jointAxis", + "parentFramePosition", + "childFramePosition", + "parentFrameOrientation", + "childFrameOrientation", + "physicsClientId", + NULL}; - - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OOi", kwlist,&parentBodyUniqueId,&parentLinkIndex, - &childBodyUniqueId,&childLinkIndex, - &jointType,&jointAxisObj, - &parentFramePositionObj, - &childFramePositionObj, - &parentFrameOrientationObj, - &childFrameOrientationObj, - &physicsClientId - )) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OOi", kwlist, &parentBodyUniqueId, &parentLinkIndex, + &childBodyUniqueId, &childLinkIndex, + &jointType, &jointAxisObj, + &parentFramePositionObj, + &childFramePositionObj, + &parentFrameOrientationObj, + &childFrameOrientationObj, + &physicsClientId)) { return NULL; } - + sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - pybullet_internalSetVectord(jointAxisObj,jointAxis); - pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition); - pybullet_internalSetVectord(childFramePositionObj,childFramePosition); - pybullet_internalSetVector4d(parentFrameOrientationObj,parentFrameOrientation); - pybullet_internalSetVector4d(childFrameOrientationObj,childFrameOrientation); - + pybullet_internalSetVectord(jointAxisObj, jointAxis); + pybullet_internalSetVectord(parentFramePositionObj, parentFramePosition); + pybullet_internalSetVectord(childFramePositionObj, childFramePosition); + pybullet_internalSetVector4d(parentFrameOrientationObj, parentFrameOrientation); + pybullet_internalSetVector4d(childFrameOrientationObj, childFrameOrientation); + jointInfo.m_jointType = jointType; jointInfo.m_parentFrame[0] = parentFramePosition[0]; jointInfo.m_parentFrame[1] = parentFramePosition[1]; @@ -3773,103 +3761,99 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P jointInfo.m_childFrame[4] = childFrameOrientation[1]; jointInfo.m_childFrame[5] = childFrameOrientation[2]; jointInfo.m_childFrame[6] = childFrameOrientation[3]; - + jointInfo.m_jointAxis[0] = jointAxis[0]; jointInfo.m_jointAxis[1] = jointAxis[1]; jointInfo.m_jointAxis[2] = jointAxis[2]; - commandHandle = b3InitCreateUserConstraintCommand(sm, parentBodyUniqueId, parentLinkIndex, childBodyUniqueId, childLinkIndex, &jointInfo); + commandHandle = b3InitCreateUserConstraintCommand(sm, parentBodyUniqueId, parentLinkIndex, childBodyUniqueId, childLinkIndex, &jointInfo); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_USER_CONSTRAINT_COMPLETED) + if (statusType == CMD_USER_CONSTRAINT_COMPLETED) { int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); PyObject* ob = PyLong_FromLong(userConstraintUid); return ob; } - PyErr_SetString(SpamError, "createConstraint failed."); return NULL; } -static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) { - int bodyUniqueIdA = -1; - int bodyUniqueIdB = -1; - - b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; - b3SharedMemoryStatusHandle statusHandle; - int statusType; +static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueIdA = -1; + int bodyUniqueIdB = -1; + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; - static char *kwlist[] = { "bodyA", "bodyB","physicsClientId", NULL }; + static char* kwlist[] = {"bodyA", "bodyB", "physicsClientId", NULL}; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iii", kwlist, - &bodyUniqueIdA, &bodyUniqueIdB,&physicsClientId)) - return NULL; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iii", kwlist, + &bodyUniqueIdA, &bodyUniqueIdB, &physicsClientId)) + return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitRequestContactPointInformation(sm); - b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA); - b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB); - //b3SetContactQueryMode(commandHandle, mode); + commandHandle = b3InitRequestContactPointInformation(sm); + b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA); + b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB); + //b3SetContactQueryMode(commandHandle, mode); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { - - b3GetContactPointInformation(sm, &contactPointData); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + b3GetContactPointInformation(sm, &contactPointData); - return MyConvertContactPoint(&contactPointData); - - } + return MyConvertContactPoint(&contactPointData); + } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } - - /// Render an image from the current timestep of the simulation, width, height are required, other args are optional // getCameraImage(w, h, view[16], projection[16], lightDir[3], lightColor[3], lightDist, hasShadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff, renderer) -static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObject* keywds) { /// request an image from a simulated camera, using software or hardware renderer. struct b3CameraImageData imageData; - PyObject* objViewMat = 0, *objProjMat = 0, *lightDirObj = 0, *lightColorObj = 0; + PyObject *objViewMat = 0, *objProjMat = 0, *lightDirObj = 0, *lightColorObj = 0; int width, height; float viewMatrix[16]; float projectionMatrix[16]; float lightDir[3]; - float lightColor[3]; - float lightDist = 10.0; - int hasShadow = 0; - float lightAmbientCoeff = 0.6; - float lightDiffuseCoeff = 0.35; - float lightSpecularCoeff = 0.05; - int renderer = 0; + float lightColor[3]; + float lightDist = 10.0; + int hasShadow = 0; + float lightAmbientCoeff = 0.6; + float lightDiffuseCoeff = 0.35; + float lightSpecularCoeff = 0.05; + int renderer = 0; // inialize cmd b3SharedMemoryCommandHandle command; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; // set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow - static char *kwlist[] = { "width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "physicsClientId", NULL }; + static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -3888,21 +3872,21 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec { b3RequestCameraImageSetLightDirection(command, lightDir); } - //set light color only if function succeeds - if (pybullet_internalSetVector(lightColorObj, lightColor)) - { - b3RequestCameraImageSetLightColor(command, lightColor); - } + //set light color only if function succeeds + if (pybullet_internalSetVector(lightColorObj, lightColor)) + { + b3RequestCameraImageSetLightColor(command, lightColor); + } - b3RequestCameraImageSetLightDistance(command, lightDist); - - b3RequestCameraImageSetShadow(command, hasShadow); - - b3RequestCameraImageSetLightAmbientCoeff(command, lightAmbientCoeff); - b3RequestCameraImageSetLightDiffuseCoeff(command, lightDiffuseCoeff); - b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff); - - b3RequestCameraImageSelectRenderer(command, renderer); + b3RequestCameraImageSetLightDistance(command, lightDist); + + b3RequestCameraImageSetShadow(command, hasShadow); + + b3RequestCameraImageSetLightAmbientCoeff(command, lightAmbientCoeff); + b3RequestCameraImageSetLightDiffuseCoeff(command, lightDiffuseCoeff); + b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff); + + b3RequestCameraImageSelectRenderer(command, renderer); if (b3CanSubmitCommand(sm)) { @@ -3913,7 +3897,8 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CAMERA_IMAGE_COMPLETED) { + if (statusType == CMD_CAMERA_IMAGE_COMPLETED) + { PyObject* item2; PyObject* pyResultList; // store 4 elements in this result: width, // height, rgbData, depth @@ -3933,26 +3918,26 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values - npy_intp rgb_dims[3] = { imageData.m_pixelHeight, imageData.m_pixelWidth, - bytesPerPixel }; - npy_intp dep_dims[2] = { imageData.m_pixelHeight, imageData.m_pixelWidth }; - npy_intp seg_dims[2] = { imageData.m_pixelHeight, imageData.m_pixelWidth }; + npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, + bytesPerPixel}; + npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; + npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8); pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32); pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32); memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData, - imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel); + imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel); memcpy(PyArray_DATA(pyDep), imageData.m_depthValues, - imageData.m_pixelHeight * imageData.m_pixelWidth); + imageData.m_pixelHeight * imageData.m_pixelWidth); memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues, - imageData.m_pixelHeight * imageData.m_pixelWidth); + imageData.m_pixelHeight * imageData.m_pixelWidth); PyTuple_SetItem(pyResultList, 2, pyRGB); PyTuple_SetItem(pyResultList, 3, pyDep); PyTuple_SetItem(pyResultList, 4, pySeg); -#else//PYBULLET_USE_NUMPY +#else //PYBULLET_USE_NUMPY PyObject* pylistRGB; PyObject* pylistDep; PyObject* pylistSeg; @@ -3975,8 +3960,10 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); pylistSeg = PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); - for (i = 0; i < imageData.m_pixelWidth; i++) { - for (j = 0; j < imageData.m_pixelHeight; j++) { + for (i = 0; i < imageData.m_pixelWidth; i++) + { + for (j = 0; j < imageData.m_pixelHeight; j++) + { // TODO(hellojas): validate depth values make sense int depIndex = i + j * imageData.m_pixelWidth; { @@ -3989,7 +3976,8 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec PyTuple_SetItem(pylistSeg, depIndex, item2); } - for (p = 0; p < bytesPerPixel; p++) { + for (p = 0; p < bytesPerPixel; p++) + { int pixelIndex = bytesPerPixel * (i + j * imageData.m_pixelWidth) + p; item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]); @@ -4003,7 +3991,7 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec PyTuple_SetItem(pyResultList, 3, pylistDep); PyTuple_SetItem(pyResultList, 4, pylistSeg); return pyResultList; -#endif//PYBULLET_USE_NUMPY +#endif //PYBULLET_USE_NUMPY return pyResultList; } @@ -4011,12 +3999,9 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec Py_INCREF(Py_None); return Py_None; - } - - -static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* camEyeObj = 0; PyObject* camTargetPositionObj = 0; @@ -4026,7 +4011,7 @@ static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyOb float camUpVector[3]; // set camera resolution, optionally view, projection matrix, light position - static char *kwlist[] = { "cameraEyePosition", "cameraTargetPosition", "cameraUpVector",NULL }; + static char* kwlist[] = {"cameraEyePosition", "cameraTargetPosition", "cameraUpVector", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOO", kwlist, &camEyeObj, &camTargetPositionObj, &camUpVectorObj)) { @@ -4038,7 +4023,7 @@ static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyOb pybullet_internalSetVector(camUpVectorObj, camUpVector)) { float viewMatrix[16]; - PyObject* pyResultList=0; + PyObject* pyResultList = 0; int i; b3ComputeViewMatrixFromPositions(camEye, camTargetPosition, camUpVector, viewMatrix); @@ -4056,7 +4041,7 @@ static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyOb } ///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices -static PyObject* pybullet_computeViewMatrixFromYawPitchRoll(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_computeViewMatrixFromYawPitchRoll(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* cameraTargetPositionObj = 0; float cameraTargetPosition[3]; @@ -4067,9 +4052,9 @@ static PyObject* pybullet_computeViewMatrixFromYawPitchRoll(PyObject* self, PyOb int i; // set camera resolution, optionally view, projection matrix, light position - static char *kwlist[] = { "cameraTargetPosition", "distance", "yaw", "pitch", "roll", "upAxisIndex" ,NULL }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "Offffi", kwlist, &cameraTargetPositionObj, &distance,&yaw,&pitch,&roll, &upAxisIndex)) + static char* kwlist[] = {"cameraTargetPosition", "distance", "yaw", "pitch", "roll", "upAxisIndex", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "Offffi", kwlist, &cameraTargetPositionObj, &distance, &yaw, &pitch, &roll, &upAxisIndex)) { return NULL; } @@ -4089,12 +4074,10 @@ static PyObject* pybullet_computeViewMatrixFromYawPitchRoll(PyObject* self, PyOb PyTuple_SetItem(pyResultList, i, item); } return pyResultList; - - } ///compute a projection matrix, helper function for b3RequestCameraImageSetCameraMatrices -static PyObject* pybullet_computeProjectionMatrix(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_computeProjectionMatrix(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* pyResultList = 0; float left; @@ -4107,14 +4090,13 @@ static PyObject* pybullet_computeProjectionMatrix(PyObject* self, PyObject* args int i; // set camera resolution, optionally view, projection matrix, light position - static char *kwlist[] = { "left", "right", "bottom", "top", "nearVal", "farVal" ,NULL }; + static char* kwlist[] = {"left", "right", "bottom", "top", "nearVal", "farVal", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ffffff", kwlist, &left, &right, &bottom, &top, &nearVal, &farVal)) { return NULL; } - b3ComputeProjectionMatrix(left, right, bottom, top, nearVal, farVal, projectionMatrix); pyResultList = PyTuple_New(16); @@ -4124,23 +4106,22 @@ static PyObject* pybullet_computeProjectionMatrix(PyObject* self, PyObject* args PyTuple_SetItem(pyResultList, i, item); } return pyResultList; - } -static PyObject* pybullet_computeProjectionMatrixFOV(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_computeProjectionMatrixFOV(PyObject* self, PyObject* args, PyObject* keywds) { - float fov, aspect, nearVal, farVal; + float fov, aspect, nearVal, farVal; PyObject* pyResultList = 0; float projectionMatrix[16]; int i; - static char *kwlist[] = { "fov","aspect","nearVal","farVal",NULL }; + static char* kwlist[] = {"fov", "aspect", "nearVal", "farVal", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ffff", kwlist, &fov, &aspect, &nearVal, &farVal)) { return NULL; } - + b3ComputeProjectionMatrixFOV(fov, aspect, nearVal, farVal, projectionMatrix); pyResultList = PyTuple_New(16); @@ -4150,10 +4131,8 @@ static PyObject* pybullet_computeProjectionMatrixFOV(PyObject* self, PyObject* a PyTuple_SetItem(pyResultList, i, item); } return pyResultList; - } - // Render an image from the current timestep of the simulation // // Examples: @@ -4177,561 +4156,620 @@ static PyObject* pybullet_computeProjectionMatrixFOV(PyObject* self, PyObject* a // TODO(hellojas): fix image is cut off at head // TODO(hellojas): should we add check to give minimum image resolution // to see object based on camera position? -static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) { - /// request an image from a simulated camera, using a software renderer. - struct b3CameraImageData imageData; - PyObject* objViewMat, *objProjMat; - PyObject* objCameraPos, *objTargetPos, *objCameraUp; +static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) +{ + /// request an image from a simulated camera, using a software renderer. + struct b3CameraImageData imageData; + PyObject *objViewMat, *objProjMat; + PyObject *objCameraPos, *objTargetPos, *objCameraUp; - int width, height; - int size = PySequence_Size(args); - float viewMatrix[16]; - float projectionMatrix[16]; + int width, height; + int size = PySequence_Size(args); + float viewMatrix[16]; + float projectionMatrix[16]; - float cameraPos[3]; - float targetPos[3]; - float cameraUp[3]; + float cameraPos[3]; + float targetPos[3]; + float cameraUp[3]; - float left, right, bottom, top, aspect; - float nearVal, farVal; - float fov; + float left, right, bottom, top, aspect; + float nearVal, farVal; + float fov; - // inialize cmd - b3SharedMemoryCommandHandle command; - b3PhysicsClientHandle sm; - int physicsClientId = 0; + // inialize cmd + b3SharedMemoryCommandHandle command; + b3PhysicsClientHandle sm; + int physicsClientId = 0; - sm = getPhysicsClient(physicsClientId); - if (sm == 0) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - command = b3InitRequestCameraImage(sm); + command = b3InitRequestCameraImage(sm); - if (size == 2) // only set camera resolution - { - if (PyArg_ParseTuple(args, "ii", &width, &height)) { - b3RequestCameraImageSetPixelResolution(command, width, height); - } - } else if (size == 4) // set camera resolution and view and projection matrix - { - if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, - &objProjMat)) { - b3RequestCameraImageSetPixelResolution(command, width, height); + if (size == 2) // only set camera resolution + { + if (PyArg_ParseTuple(args, "ii", &width, &height)) + { + b3RequestCameraImageSetPixelResolution(command, width, height); + } + } + else if (size == 4) // set camera resolution and view and projection matrix + { + if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, + &objProjMat)) + { + b3RequestCameraImageSetPixelResolution(command, width, height); - // set camera matrices only if set matrix function succeeds - if (pybullet_internalSetMatrix(objViewMat, viewMatrix) && - (pybullet_internalSetMatrix(objProjMat, projectionMatrix))) { - b3RequestCameraImageSetCameraMatrices(command, viewMatrix, - projectionMatrix); - } else { - PyErr_SetString(SpamError, "Error parsing view or projection matrix."); - return NULL; - } - } - } else if (size == 7) // set camera resolution, camera positions and - // calculate projection using near/far values. - { - if (PyArg_ParseTuple(args, "iiOOOff", &width, &height, &objCameraPos, - &objTargetPos, &objCameraUp, &nearVal, &farVal)) { - b3RequestCameraImageSetPixelResolution(command, width, height); - if (pybullet_internalSetVector(objCameraPos, cameraPos) && - pybullet_internalSetVector(objTargetPos, targetPos) && - pybullet_internalSetVector(objCameraUp, cameraUp)) - { - b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, - cameraUp); - } else { - PyErr_SetString(SpamError, - "Error parsing camera position, target or up."); - return NULL; - } + // set camera matrices only if set matrix function succeeds + if (pybullet_internalSetMatrix(objViewMat, viewMatrix) && + (pybullet_internalSetMatrix(objProjMat, projectionMatrix))) + { + b3RequestCameraImageSetCameraMatrices(command, viewMatrix, + projectionMatrix); + } + else + { + PyErr_SetString(SpamError, "Error parsing view or projection matrix."); + return NULL; + } + } + } + else if (size == 7) // set camera resolution, camera positions and + // calculate projection using near/far values. + { + if (PyArg_ParseTuple(args, "iiOOOff", &width, &height, &objCameraPos, + &objTargetPos, &objCameraUp, &nearVal, &farVal)) + { + b3RequestCameraImageSetPixelResolution(command, width, height); + if (pybullet_internalSetVector(objCameraPos, cameraPos) && + pybullet_internalSetVector(objTargetPos, targetPos) && + pybullet_internalSetVector(objCameraUp, cameraUp)) + { + b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, + cameraUp); + } + else + { + PyErr_SetString(SpamError, + "Error parsing camera position, target or up."); + return NULL; + } - aspect = width / height; - left = -aspect * nearVal; - right = aspect * nearVal; - bottom = -nearVal; - top = nearVal; - b3RequestCameraImageSetProjectionMatrix(command, left, right, bottom, top, - nearVal, farVal); - } - } else if (size == 8) // set camera resolution, camera positions and - // calculate projection using near/far values & field - // of view - { - if (PyArg_ParseTuple(args, "iiOOOfff", &width, &height, &objCameraPos, - &objTargetPos, &objCameraUp, &nearVal, &farVal, - &fov)) { - b3RequestCameraImageSetPixelResolution(command, width, height); - if (pybullet_internalSetVector(objCameraPos, cameraPos) && - pybullet_internalSetVector(objTargetPos, targetPos) && - pybullet_internalSetVector(objCameraUp, cameraUp)) { - b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, - cameraUp); - } else { - PyErr_SetString(SpamError, - "Error parsing camera position, target or up."); - return NULL; - } + aspect = width / height; + left = -aspect * nearVal; + right = aspect * nearVal; + bottom = -nearVal; + top = nearVal; + b3RequestCameraImageSetProjectionMatrix(command, left, right, bottom, top, + nearVal, farVal); + } + } + else if (size == 8) // set camera resolution, camera positions and + // calculate projection using near/far values & field + // of view + { + if (PyArg_ParseTuple(args, "iiOOOfff", &width, &height, &objCameraPos, + &objTargetPos, &objCameraUp, &nearVal, &farVal, + &fov)) + { + b3RequestCameraImageSetPixelResolution(command, width, height); + if (pybullet_internalSetVector(objCameraPos, cameraPos) && + pybullet_internalSetVector(objTargetPos, targetPos) && + pybullet_internalSetVector(objCameraUp, cameraUp)) + { + b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, + cameraUp); + } + else + { + PyErr_SetString(SpamError, + "Error parsing camera position, target or up."); + return NULL; + } - aspect = width / height; - b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, - farVal); - } - } else if (size == 11) { - int upAxisIndex = 1; - float camDistance, yaw, pitch, roll; + aspect = width / height; + b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, + farVal); + } + } + else if (size == 11) + { + int upAxisIndex = 1; + float camDistance, yaw, pitch, roll; - // sometimes more arguments are better :-) - 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); + // sometimes more arguments are better :-) + 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, roll, upAxisIndex); - aspect = width / height; - b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, - nearVal, farVal); - } else { - PyErr_SetString(SpamError, "Error parsing camera target pos"); - } - } else { - PyErr_SetString(SpamError, "Error parsing arguments"); - } + b3RequestCameraImageSetViewMatrix2(command, targetPos, camDistance, yaw, + pitch, roll, upAxisIndex); + aspect = width / height; + b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, + nearVal, farVal); + } + else + { + PyErr_SetString(SpamError, "Error parsing camera target pos"); + } + } + else + { + PyErr_SetString(SpamError, "Error parsing arguments"); + } + } + else + { + PyErr_SetString(SpamError, "Invalid number of args passed to renderImage."); + return NULL; + } - } else { - PyErr_SetString(SpamError, "Invalid number of args passed to renderImage."); - return NULL; - } + if (b3CanSubmitCommand(sm)) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; - if (b3CanSubmitCommand(sm)) { - b3SharedMemoryStatusHandle statusHandle; - int statusType; + // b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); - // b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CAMERA_IMAGE_COMPLETED) { - PyObject* item2; - PyObject* pyResultList; // store 4 elements in this result: width, - // height, rgbData, depth + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CAMERA_IMAGE_COMPLETED) + { + PyObject* item2; + PyObject* pyResultList; // store 4 elements in this result: width, + // height, rgbData, depth #ifdef PYBULLET_USE_NUMPY - PyObject* pyRGB; - PyObject* pyDep; - PyObject* pySeg; + PyObject* pyRGB; + PyObject* pyDep; + PyObject* pySeg; - int i, j, p; + int i, j, p; - b3GetCameraImageData(sm, &imageData); - // TODO(hellojas): error handling if image size is 0 - pyResultList = PyTuple_New(5); - PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); - PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); + b3GetCameraImageData(sm, &imageData); + // TODO(hellojas): error handling if image size is 0 + pyResultList = PyTuple_New(5); + PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); + PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); - int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values + int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values - npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, - bytesPerPixel}; - npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; - npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; + npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, + bytesPerPixel}; + npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; + npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; - pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8); - pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32); - pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32); + pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8); + pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32); + pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32); - memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData, - imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel); - memcpy(PyArray_DATA(pyDep), imageData.m_depthValues, - imageData.m_pixelHeight * imageData.m_pixelWidth); - memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues, - imageData.m_pixelHeight * imageData.m_pixelWidth); - - PyTuple_SetItem(pyResultList, 2, pyRGB); - PyTuple_SetItem(pyResultList, 3, pyDep); - PyTuple_SetItem(pyResultList, 4, pySeg); -#else//PYBULLET_USE_NUMPY - PyObject* pylistRGB; - PyObject* pylistDep; - PyObject* pylistSeg; + memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData, + imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel); + memcpy(PyArray_DATA(pyDep), imageData.m_depthValues, + imageData.m_pixelHeight * imageData.m_pixelWidth); + memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues, + imageData.m_pixelHeight * imageData.m_pixelWidth); - int i, j, p; + PyTuple_SetItem(pyResultList, 2, pyRGB); + PyTuple_SetItem(pyResultList, 3, pyDep); + PyTuple_SetItem(pyResultList, 4, pySeg); +#else //PYBULLET_USE_NUMPY + PyObject* pylistRGB; + PyObject* pylistDep; + PyObject* pylistSeg; - b3GetCameraImageData(sm, &imageData); - // TODO(hellojas): error handling if image size is 0 - pyResultList = PyTuple_New(5); - PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); - PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); + int i, j, p; - { - PyObject* item; - int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values - int num = - bytesPerPixel * imageData.m_pixelWidth * imageData.m_pixelHeight; - pylistRGB = PyTuple_New(num); - pylistDep = - PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); - pylistSeg = - PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); - for (i = 0; i < imageData.m_pixelWidth; i++) { - for (j = 0; j < imageData.m_pixelHeight; j++) { - // TODO(hellojas): validate depth values make sense - int depIndex = i + j * imageData.m_pixelWidth; - { - item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]); - PyTuple_SetItem(pylistDep, depIndex, item); - } - { - item2 = - PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]); - PyTuple_SetItem(pylistSeg, depIndex, item2); - } + b3GetCameraImageData(sm, &imageData); + // TODO(hellojas): error handling if image size is 0 + pyResultList = PyTuple_New(5); + PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); + PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); - for (p = 0; p < bytesPerPixel; p++) { - int pixelIndex = - bytesPerPixel * (i + j * imageData.m_pixelWidth) + p; - item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]); - PyTuple_SetItem(pylistRGB, pixelIndex, item); - } - } - } - } + { + PyObject* item; + int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values + int num = + bytesPerPixel * imageData.m_pixelWidth * imageData.m_pixelHeight; + pylistRGB = PyTuple_New(num); + pylistDep = + PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); + pylistSeg = + PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); + for (i = 0; i < imageData.m_pixelWidth; i++) + { + for (j = 0; j < imageData.m_pixelHeight; j++) + { + // TODO(hellojas): validate depth values make sense + int depIndex = i + j * imageData.m_pixelWidth; + { + item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]); + PyTuple_SetItem(pylistDep, depIndex, item); + } + { + item2 = + PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]); + PyTuple_SetItem(pylistSeg, depIndex, item2); + } - PyTuple_SetItem(pyResultList, 2, pylistRGB); - PyTuple_SetItem(pyResultList, 3, pylistDep); - PyTuple_SetItem(pyResultList, 4, pylistSeg); - return pyResultList; -#endif//PYBULLET_USE_NUMPY + for (p = 0; p < bytesPerPixel; p++) + { + int pixelIndex = + bytesPerPixel * (i + j * imageData.m_pixelWidth) + p; + item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]); + PyTuple_SetItem(pylistRGB, pixelIndex, item); + } + } + } + } - return pyResultList; - } - } + PyTuple_SetItem(pyResultList, 2, pylistRGB); + PyTuple_SetItem(pyResultList, 3, pylistDep); + PyTuple_SetItem(pyResultList, 4, pylistSeg); + return pyResultList; +#endif //PYBULLET_USE_NUMPY - Py_INCREF(Py_None); - return Py_None; + return pyResultList; + } + } + + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args,PyObject* keywds) { - - { - int objectUniqueId=-1, linkIndex=-1, flags; - double force[3]; - double position[3] = {0.0, 0.0, 0.0}; - PyObject* forceObj=0, *posObj=0; - - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "objectUniqueId", "linkIndex", - "forceObj", "posObj", "flags", "physicsClientId", NULL }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOOi|i", kwlist,&objectUniqueId, &linkIndex, - &forceObj, &posObj, &flags, &physicsClientId)) +static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args, PyObject* keywds) +{ { - return NULL; + int objectUniqueId = -1, linkIndex = -1, flags; + double force[3]; + double position[3] = {0.0, 0.0, 0.0}; + PyObject *forceObj = 0, *posObj = 0; + + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"objectUniqueId", "linkIndex", + "forceObj", "posObj", "flags", "physicsClientId", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOOi|i", kwlist, &objectUniqueId, &linkIndex, + &forceObj, &posObj, &flags, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(forceObj, "expected a sequence"); + len = PySequence_Size(forceObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + force[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "force needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(posObj, "expected a sequence"); + len = PySequence_Size(posObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + position[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) + { + PyErr_SetString(SpamError, + "flag has to be either WORLD_FRAME or LINK_FRAME"); + return NULL; + } + command = b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, + flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(forceObj, "expected a sequence"); - len = PySequence_Size(forceObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - force[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "force needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(posObj, "expected a sequence"); - len = PySequence_Size(posObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - position[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) { - PyErr_SetString(SpamError, - "flag has to be either WORLD_FRAME or LINK_FRAME"); - return NULL; - } - command = b3ApplyExternalForceCommandInit(sm); - b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, - flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args,PyObject* keywds) { - - { - int objectUniqueId, linkIndex, flags; - double torque[3]; - PyObject* torqueObj; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "objectUniqueId", "linkIndex", "torqueObj", - "flags", "physicsClientId", NULL }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOi|i", kwlist,&objectUniqueId, &linkIndex, &torqueObj, - &flags, &physicsClientId)) +static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args, PyObject* keywds) +{ { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(torqueObj, "expected a sequence"); - len = PySequence_Size(torqueObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - torque[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "torque needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); + int objectUniqueId, linkIndex, flags; + double torque[3]; + PyObject* torqueObj; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"objectUniqueId", "linkIndex", "torqueObj", + "flags", "physicsClientId", NULL}; - if (linkIndex < -1) { - PyErr_SetString(SpamError, - "Invalid link index, has to be -1 or larger"); - return NULL; - } - if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) { - PyErr_SetString(SpamError, - "flag has to be either WORLD_FRAME or LINK_FRAME"); - return NULL; - } - { - b3SharedMemoryStatusHandle statusHandle; - b3SharedMemoryCommandHandle command = - b3ApplyExternalForceCommandInit(sm); - b3ApplyExternalTorque(command, objectUniqueId, -1, torque, flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - } - } - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOi|i", kwlist, &objectUniqueId, &linkIndex, &torqueObj, + &flags, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - Py_INCREF(Py_None); - return Py_None; + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(torqueObj, "expected a sequence"); + len = PySequence_Size(torqueObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + torque[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "torque needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + + if (linkIndex < -1) + { + PyErr_SetString(SpamError, + "Invalid link index, has to be -1 or larger"); + return NULL; + } + if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) + { + PyErr_SetString(SpamError, + "flag has to be either WORLD_FRAME or LINK_FRAME"); + return NULL; + } + { + b3SharedMemoryStatusHandle statusHandle; + b3SharedMemoryCommandHandle command = + b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalTorque(command, objectUniqueId, -1, torque, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } + } + } + + Py_INCREF(Py_None); + return Py_None; } static PyObject* pybullet_getQuaternionFromEuler(PyObject* self, - PyObject* args) { - double rpy[3]; + PyObject* args) +{ + double rpy[3]; - PyObject* eulerObj; + PyObject* eulerObj; - if (PyArg_ParseTuple(args, "O", &eulerObj)) { - PyObject* seq; - int len, i; - seq = PySequence_Fast(eulerObj, "expected a sequence"); - len = PySequence_Size(eulerObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - rpy[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, - "Euler angles need a 3 coordinates [roll, pitch, yaw]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } else { - PyErr_SetString(SpamError, - "Euler angles need a 3 coordinates [roll, pitch, yaw]."); - return NULL; - } + if (PyArg_ParseTuple(args, "O", &eulerObj)) + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(eulerObj, "expected a sequence"); + len = PySequence_Size(eulerObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + rpy[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, + "Euler angles need a 3 coordinates [roll, pitch, yaw]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + else + { + PyErr_SetString(SpamError, + "Euler angles need a 3 coordinates [roll, pitch, yaw]."); + return NULL; + } - { - double phi, the, psi; - double roll = rpy[0]; - double pitch = rpy[1]; - double yaw = rpy[2]; - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - { - double quat[4] = { - sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), - cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), - cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), - cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)}; + { + double phi, the, psi; + double roll = rpy[0]; + double pitch = rpy[1]; + double yaw = rpy[2]; + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 2.0; + { + double quat[4] = { + sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), + cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), + cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), + cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)}; - // normalize the quaternion - double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] + - quat[2] * quat[2] + quat[3] * quat[3]); - quat[0] /= len; - quat[1] /= len; - quat[2] /= len; - quat[3] /= len; - { - PyObject* pylist; - int i; - pylist = PyTuple_New(4); - for (i = 0; i < 4; i++) - PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quat[i])); - return pylist; - } - } - } - Py_INCREF(Py_None); - return Py_None; + // normalize the quaternion + double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] + + quat[2] * quat[2] + quat[3] * quat[3]); + quat[0] /= len; + quat[1] /= len; + quat[2] /= len; + quat[3] /= len; + { + PyObject* pylist; + int i; + pylist = PyTuple_New(4); + for (i = 0; i < 4; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quat[i])); + return pylist; + } + } + } + Py_INCREF(Py_None); + return Py_None; } /// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo /// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, - PyObject* args) { - double squ; - double sqx; - double sqy; - double sqz; + PyObject* args) +{ + double squ; + double sqx; + double sqy; + double sqz; - double quat[4]; + double quat[4]; - PyObject* quatObj; + PyObject* quatObj; - if (PyArg_ParseTuple(args, "O", &quatObj)) { - PyObject* seq; - int len, i; - seq = PySequence_Fast(quatObj, "expected a sequence"); - len = PySequence_Size(quatObj); - if (len == 4) { - for (i = 0; i < 4; i++) { - quat[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } else { - PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); - return NULL; - } + if (PyArg_ParseTuple(args, "O", &quatObj)) + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(quatObj, "expected a sequence"); + len = PySequence_Size(quatObj); + if (len == 4) + { + for (i = 0; i < 4; i++) + { + quat[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + return NULL; + } - { - double rpy[3]; - double sarg; - sqx = quat[0] * quat[0]; - sqy = quat[1] * quat[1]; - sqz = quat[2] * quat[2]; - squ = quat[3] * quat[3]; - rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), - squ - sqx - sqy + sqz); - sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]); - rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538 - : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg)); - rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), - squ + sqx - sqy - sqz); - { - PyObject* pylist; - int i; - pylist = PyTuple_New(3); - for (i = 0; i < 3; i++) - PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i])); - return pylist; - } - } - Py_INCREF(Py_None); - return Py_None; + { + double rpy[3]; + double sarg; + sqx = quat[0] * quat[0]; + sqy = quat[1] * quat[1]; + sqz = quat[2] * quat[2]; + squ = quat[3] * quat[3]; + rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), + squ - sqx - sqy + sqz); + sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]); + rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538 + : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg)); + rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), + squ + sqx - sqy - sqz); + { + PyObject* pylist; + int i; + pylist = PyTuple_New(3); + for (i = 0; i < 3; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i])); + return pylist; + } + } + Py_INCREF(Py_None); + return Py_None; } - ///Inverse Kinematics binding static PyObject* pybullet_calculateInverseKinematics(PyObject* self, - PyObject* args, PyObject* keywds) + PyObject* args, PyObject* keywds) - { - int bodyIndex; - int endEffectorLinkIndex; - - PyObject* targetPosObj=0; - PyObject* targetOrnObj=0; +{ + int bodyIndex; + int endEffectorLinkIndex; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - PyObject* lowerLimitsObj = 0; - PyObject* upperLimitsObj = 0; - PyObject* jointRangesObj = 0; - PyObject* restPosesObj = 0; - PyObject* jointDampingObj = 0; + PyObject* targetPosObj = 0; + PyObject* targetOrnObj = 0; - static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", "jointDamping", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOiO", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId, & jointDampingObj)) + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + PyObject* lowerLimitsObj = 0; + PyObject* upperLimitsObj = 0; + PyObject* jointRangesObj = 0; + PyObject* restPosesObj = 0; + PyObject* jointDampingObj = 0; + + static char* kwlist[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", "jointDamping", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOiO", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId, &jointDampingObj)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } { double pos[3]; - double ori[4]={0,1.0,0,0}; - int hasPos =pybullet_internalSetVectord(targetPosObj,pos); - int hasOrn = pybullet_internalSetVector4d(targetOrnObj,ori); + double ori[4] = {0, 1.0, 0, 0}; + int hasPos = pybullet_internalSetVectord(targetPosObj, pos); + int hasOrn = pybullet_internalSetVector4d(targetOrnObj, ori); int szLowerLimits = lowerLimitsObj ? PySequence_Size(lowerLimitsObj) : 0; - int szUpperLimits = upperLimitsObj? PySequence_Size(upperLimitsObj): 0; - int szJointRanges = jointRangesObj? PySequence_Size(jointRangesObj):0; - int szRestPoses = restPosesObj? PySequence_Size(restPosesObj):0; - int szJointDamping = jointDampingObj? PySequence_Size(jointDampingObj):0; + int szUpperLimits = upperLimitsObj ? PySequence_Size(upperLimitsObj) : 0; + int szJointRanges = jointRangesObj ? PySequence_Size(jointRangesObj) : 0; + int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0; + int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0; int numJoints = b3GetNumJoints(sm, bodyIndex); - int hasNullSpace = 0; - int hasJointDamping = 0; + int hasJointDamping = 0; double* lowerLimits = 0; double* upperLimits = 0; double* jointRanges = 0; double* restPoses = 0; - double* jointDamping = 0; - + double* jointDamping = 0; + if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && - (szJointRanges == numJoints) && (szRestPoses == numJoints)) + (szJointRanges == numJoints) && (szRestPoses == numJoints)) { int szInBytes = sizeof(double) * numJoints; int i; @@ -4740,77 +4778,80 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, jointRanges = (double*)malloc(szInBytes); restPoses = (double*)malloc(szInBytes); - for (i = 0; i < numJoints; i++) + for (i = 0; i < numJoints; i++) { - lowerLimits[i] = - pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); - upperLimits[i] = - pybullet_internalGetFloatFromSequence(upperLimitsObj, i); - jointRanges[i] = - pybullet_internalGetFloatFromSequence(jointRangesObj, i); - restPoses[i] = - pybullet_internalGetFloatFromSequence(restPosesObj, i); + lowerLimits[i] = + pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); + upperLimits[i] = + pybullet_internalGetFloatFromSequence(upperLimitsObj, i); + jointRanges[i] = + pybullet_internalGetFloatFromSequence(jointRangesObj, i); + restPoses[i] = + pybullet_internalGetFloatFromSequence(restPosesObj, i); } hasNullSpace = 1; } - - if (numJoints && (szJointDamping == numJoints)) - { - int szInBytes = sizeof(double) * numJoints; - int i; - jointDamping = (double*)malloc(szInBytes); - for (i = 0; i < numJoints; i++) - { - jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); - } - hasJointDamping = 1; - } + + if (numJoints && (szJointDamping == numJoints)) + { + int szInBytes = sizeof(double) * numJoints; + int i; + jointDamping = (double*)malloc(szInBytes); + for (i = 0; i < numJoints; i++) + { + jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); + } + hasJointDamping = 1; + } if (hasPos) { b3SharedMemoryStatusHandle statusHandle; - int numPos=0; + int numPos = 0; int resultBodyIndex; int result; - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex); + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyIndex); if (hasNullSpace) { if (hasOrn) { b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); - } else - { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command,numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); } - } else + else + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); + } + } + else { if (hasOrn) { - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,endEffectorLinkIndex,pos,ori); - } else + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, endEffectorLinkIndex, pos, ori); + } + else { - b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos); + b3CalculateInverseKinematicsAddTargetPurePosition(command, endEffectorLinkIndex, pos); } } - - if (hasJointDamping) - { - b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); - } + + if (hasJointDamping) + { + b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &resultBodyIndex, - &numPos, - 0); + &resultBodyIndex, + &numPos, + 0); if (result && numPos) { int i; PyObject* pylist; - double* ikOutPutJointPos = (double*)malloc(numPos*sizeof(double)); + double* ikOutPutJointPos = (double*)malloc(numPos * sizeof(double)); result = b3GetStatusInverseKinematicsJointPositions(statusHandle, &resultBodyIndex, &numPos, @@ -4821,7 +4862,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, PyTuple_SetItem(pylist, i, PyFloat_FromDouble(ikOutPutJointPos[i])); } - + free(ikOutPutJointPos); return pylist; } @@ -4831,440 +4872,408 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, "Error in calculateInverseKinematics"); return NULL; } - } else + } + else { PyErr_SetString(SpamError, - "calculateInverseKinematics couldn't extract position vector3"); + "calculateInverseKinematics couldn't extract position vector3"); return NULL; } } - - Py_INCREF(Py_None); - return Py_None; + + Py_INCREF(Py_None); + 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,PyObject* keywds) + PyObject* args, PyObject* keywds) { - - - { - int bodyIndex; - PyObject* objPositionsQ; - PyObject* objVelocitiesQdot; - PyObject* objAccelerations; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyIndex", "objPositions", "objVelocities", "objAccelerations","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyIndex, &objPositionsQ, - &objVelocitiesQdot, &objAccelerations,&physicsClientId)) { - return NULL; + int bodyIndex; + PyObject* objPositionsQ; + PyObject* objVelocitiesQdot; + PyObject* objAccelerations; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"bodyIndex", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyIndex, &objPositionsQ, + &objVelocitiesQdot, &objAccelerations, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + 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; + } + } } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - 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; - } - - } - } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } static PyMethodDef SpamMethods[] = { - {"connect",(PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS|METH_KEYWORDS, - "Connect to an existing physics server (using shared memory by default)."}, + {"connect", (PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS | METH_KEYWORDS, + "Connect to an existing physics server (using shared memory by default)."}, - {"disconnect", (PyCFunction)pybullet_disconnectPhysicsServer, METH_VARARGS|METH_KEYWORDS, - "Disconnect from the physics server."}, + {"disconnect", (PyCFunction)pybullet_disconnectPhysicsServer, METH_VARARGS | METH_KEYWORDS, + "Disconnect from the physics server."}, - {"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS|METH_KEYWORDS, - "Reset the simulation: remove all objects and start from an empty world."}, + {"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS, + "Reset the simulation: remove all objects and start from an empty world."}, - {"stepSimulation", (PyCFunction)pybullet_stepSimulation, METH_VARARGS|METH_KEYWORDS, - "Step the simulation using forward dynamics."}, + {"stepSimulation", (PyCFunction)pybullet_stepSimulation, METH_VARARGS | METH_KEYWORDS, + "Step the simulation using forward dynamics."}, - {"setGravity", (PyCFunction)pybullet_setGravity, METH_VARARGS|METH_KEYWORDS, - "Set the gravity acceleration (x,y,z)."}, + {"setGravity", (PyCFunction)pybullet_setGravity, METH_VARARGS | METH_KEYWORDS, + "Set the gravity acceleration (x,y,z)."}, - {"setTimeStep",(PyCFunction) pybullet_setTimeStep, METH_VARARGS|METH_KEYWORDS, - "Set the amount of time to proceed at each call to stepSimulation. (unit " - "is seconds, typically range is 0.01 or 0.001)"}, + {"setTimeStep", (PyCFunction)pybullet_setTimeStep, METH_VARARGS | METH_KEYWORDS, + "Set the amount of time to proceed at each call to stepSimulation. (unit " + "is seconds, typically range is 0.01 or 0.001)"}, - - - {"setDefaultContactERP", (PyCFunction) pybullet_setDefaultContactERP, METH_VARARGS| METH_KEYWORDS, - "Set the amount of contact penetration Error Recovery Paramater " - "(ERP) in each time step. \ + {"setDefaultContactERP", (PyCFunction)pybullet_setDefaultContactERP, METH_VARARGS | METH_KEYWORDS, + "Set the amount of contact penetration Error Recovery Paramater " + "(ERP) in each time step. \ This is an tuning parameter to control resting contact stability. " - "This value depends on the time step."}, - - { "setRealTimeSimulation",(PyCFunction) pybullet_setRealTimeSimulation, METH_VARARGS| METH_KEYWORDS, - "Enable or disable real time simulation (using the real time clock," - " RTC) in the physics server. Expects one integer argument, 0 or 1" }, + "This value depends on the time step."}, - { "setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS, - "Set some internal physics engine parameter, such as cfm or erp etc." }, + {"setRealTimeSimulation", (PyCFunction)pybullet_setRealTimeSimulation, METH_VARARGS | METH_KEYWORDS, + "Enable or disable real time simulation (using the real time clock," + " RTC) in the physics server. Expects one integer argument, 0 or 1"}, - { "setInternalSimFlags", (PyCFunction)pybullet_setInternalSimFlags, METH_VARARGS| METH_KEYWORDS, - "This is for experimental purposes, use at own risk, magic may or not happen"}, - - {"loadURDF", (PyCFunction) pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS, - "Create a multibody by loading a URDF file."}, + {"setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS, + "Set some internal physics engine parameter, such as cfm or erp etc."}, - {"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS| METH_KEYWORDS, - "Load multibodies from an SDF file."}, + {"setInternalSimFlags", (PyCFunction)pybullet_setInternalSimFlags, METH_VARARGS | METH_KEYWORDS, + "This is for experimental purposes, use at own risk, magic may or not happen"}, - { "loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS| METH_KEYWORDS, - "Restore the full state of the world from a .bullet file." }, + {"loadURDF", (PyCFunction)pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS, + "Create a multibody by loading a URDF file."}, - { "saveBullet", (PyCFunction)pybullet_saveBullet, METH_VARARGS| METH_KEYWORDS, - "Save the full state of the world to a .bullet file." }, + {"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS | METH_KEYWORDS, + "Load multibodies from an SDF file."}, - { "loadMJCF",(PyCFunction) pybullet_loadMJCF, METH_VARARGS| METH_KEYWORDS, - "Load multibodies from an MJCF file." }, + {"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS, + "Restore the full state of the world from a .bullet file."}, - {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, - "Create a constraint between two bodies. Returns a (int) unique id, if successfull." - }, + {"saveBullet", (PyCFunction)pybullet_saveBullet, METH_VARARGS | METH_KEYWORDS, + "Save the full state of the world to a .bullet file."}, + + {"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS, + "Load multibodies from an MJCF file."}, + + {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, + "Create a constraint between two bodies. Returns a (int) unique id, if successfull."}, {"changeConstraint", (PyCFunction)pybullet_changeUserConstraint, METH_VARARGS | METH_KEYWORDS, - "Change some parameters of an existing constraint, such as the child pivot or child frame orientation, using its unique id." - }, - + "Change some parameters of an existing constraint, such as the child pivot or child frame orientation, using its unique id."}, {"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS, - "Remove a constraint using its unique id." - }, - - { "enableJointForceTorqueSensor", (PyCFunction)pybullet_enableJointForceTorqueSensor, METH_VARARGS | METH_KEYWORDS, - "Enable or disable a joint force/torque sensor measuring the joint reaction forces." - }, + "Remove a constraint using its unique id."}, - {"saveWorld", (PyCFunction)pybullet_saveWorld, METH_VARARGS| METH_KEYWORDS, - "Save a approximate Python file to reproduce the current state of the world: saveWorld" - "(filename). (very preliminary and approximately)"}, + {"enableJointForceTorqueSensor", (PyCFunction)pybullet_enableJointForceTorqueSensor, METH_VARARGS | METH_KEYWORDS, + "Enable or disable a joint force/torque sensor measuring the joint reaction forces."}, - {"getNumBodies", (PyCFunction)pybullet_getNumBodies, METH_VARARGS| METH_KEYWORDS, - "Get the number of bodies in the simulation."}, + {"saveWorld", (PyCFunction)pybullet_saveWorld, METH_VARARGS | METH_KEYWORDS, + "Save a approximate Python file to reproduce the current state of the world: saveWorld" + "(filename). (very preliminary and approximately)"}, - {"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS, - "getBodyUniqueId is used after connecting to server with existing bodies." + {"getNumBodies", (PyCFunction)pybullet_getNumBodies, METH_VARARGS | METH_KEYWORDS, + "Get the number of bodies in the simulation."}, + + {"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS | METH_KEYWORDS, + "getBodyUniqueId is used after connecting to server with existing bodies." "Get the unique id of the body, given a integer range [0.. number of bodies)."}, - {"getBodyInfo",(PyCFunction) pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS, - "Get the body info, given a body unique id."}, - - {"getNumConstraints", (PyCFunction)pybullet_getNumConstraints, METH_VARARGS| METH_KEYWORDS, - "Get the number of user-created constraints in the simulation."}, + {"getBodyInfo", (PyCFunction)pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS, + "Get the body info, given a body unique id."}, - {"getConstraintInfo",(PyCFunction) pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS, - "Get the user-created constraint info, given a constraint unique id."}, - - {"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS, - "Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."}, - - {"getBasePositionAndOrientation",(PyCFunction) pybullet_getBasePositionAndOrientation, - METH_VARARGS | METH_KEYWORDS, - "Get the world position and orientation of the base of the object. " - "(x,y,z) position vector and (x,y,z,w) quaternion orientation."}, + {"getNumConstraints", (PyCFunction)pybullet_getNumConstraints, METH_VARARGS | METH_KEYWORDS, + "Get the number of user-created constraints in the simulation."}, - {"resetBasePositionAndOrientation", - (PyCFunction) pybullet_resetBasePositionAndOrientation, METH_VARARGS| METH_KEYWORDS, - "Reset the world position and orientation of the base of the object " - "instantaneously, not through physics simulation. (x,y,z) position vector " - "and (x,y,z,w) quaternion orientation."}, + {"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS, + "Get the user-created constraint info, given a constraint unique id."}, - { "getBaseVelocity", (PyCFunction)pybullet_getBaseVelocity, - METH_VARARGS| METH_KEYWORDS, - "Get the linear and angular velocity of the base of the object " - " in world space coordinates. " - "(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." }, + {"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS | METH_KEYWORDS, + "Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."}, - { "resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS, - "Reset the linear and/or angular velocity of the base of the object " - " in world space coordinates. " - "linearVelocity (x,y,z) and angularVelocity (x,y,z)." }, + {"getBasePositionAndOrientation", (PyCFunction)pybullet_getBasePositionAndOrientation, + METH_VARARGS | METH_KEYWORDS, + "Get the world position and orientation of the base of the object. " + "(x,y,z) position vector and (x,y,z,w) quaternion orientation."}, - {"getNumJoints", (PyCFunction)pybullet_getNumJoints, METH_VARARGS| METH_KEYWORDS, - "Get the number of joints for an object."}, + {"resetBasePositionAndOrientation", + (PyCFunction)pybullet_resetBasePositionAndOrientation, METH_VARARGS | METH_KEYWORDS, + "Reset the world position and orientation of the base of the object " + "instantaneously, not through physics simulation. (x,y,z) position vector " + "and (x,y,z,w) quaternion orientation."}, - {"getJointInfo", (PyCFunction)pybullet_getJointInfo, METH_VARARGS| METH_KEYWORDS, - "Get the name and type info for a joint on a body."}, + {"getBaseVelocity", (PyCFunction)pybullet_getBaseVelocity, + METH_VARARGS | METH_KEYWORDS, + "Get the linear and angular velocity of the base of the object " + " in world space coordinates. " + "(x,y,z) linear velocity vector and (x,y,z) angular velocity vector."}, - {"getJointState",(PyCFunction) pybullet_getJointState, METH_VARARGS| METH_KEYWORDS, - "Get the state (position, velocity etc) for a joint on a body."}, + {"resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS, + "Reset the linear and/or angular velocity of the base of the object " + " in world space coordinates. " + "linearVelocity (x,y,z) and angularVelocity (x,y,z)."}, - {"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS| METH_KEYWORDS, - "Provides extra information such as the Cartesian world coordinates" - " center of mass (COM) of the link, relative to the world reference" - " frame."}, + {"getNumJoints", (PyCFunction)pybullet_getNumJoints, METH_VARARGS | METH_KEYWORDS, + "Get the number of joints for an object."}, - {"resetJointState",(PyCFunction) pybullet_resetJointState, METH_VARARGS| METH_KEYWORDS, - "Reset the state (position, velocity etc) for a joint on a body " - "instantaneously, not through physics simulation."}, + {"getJointInfo", (PyCFunction)pybullet_getJointInfo, METH_VARARGS | METH_KEYWORDS, + "Get the name and type info for a joint on a body."}, - {"setJointMotorControl",(PyCFunction) pybullet_setJointMotorControl, METH_VARARGS, - "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead." + {"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS, + "Get the state (position, velocity etc) for a joint on a body."}, + + {"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS, + "Provides extra information such as the Cartesian world coordinates" + " center of mass (COM) of the link, relative to the world reference" + " frame."}, + + {"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS, + "Reset the state (position, velocity etc) for a joint on a body " + "instantaneously, not through physics simulation."}, + + {"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS, + "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead." "Set a single joint motor control mode and desired target value. There is " - "no immediate state change, stepSimulation will process the motors."}, + "no immediate state change, stepSimulation will process the motors."}, - {"setJointMotorControl2",(PyCFunction) pybullet_setJointMotorControl2, METH_VARARGS| METH_KEYWORDS, - "Set a single joint motor control mode and desired target value. There is " - "no immediate state change, stepSimulation will process the motors."}, + {"setJointMotorControl2", (PyCFunction)pybullet_setJointMotorControl2, METH_VARARGS | METH_KEYWORDS, + "Set a single joint motor control mode and desired target value. There is " + "no immediate state change, stepSimulation will process the motors."}, - {"applyExternalForce",(PyCFunction) pybullet_applyExternalForce, METH_VARARGS| METH_KEYWORDS, - "for objectUniqueId, linkIndex (-1 for base/root link), apply a force " - "[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or " - "WORLD_FRAME coordinates"}, + {"applyExternalForce", (PyCFunction)pybullet_applyExternalForce, METH_VARARGS | METH_KEYWORDS, + "for objectUniqueId, linkIndex (-1 for base/root link), apply a force " + "[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or " + "WORLD_FRAME coordinates"}, - {"applyExternalTorque", (PyCFunction)pybullet_applyExternalTorque, METH_VARARGS| METH_KEYWORDS, - "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque " - "[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or " - "WORLD_FRAME coordinates"}, + {"applyExternalTorque", (PyCFunction)pybullet_applyExternalTorque, METH_VARARGS | METH_KEYWORDS, + "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque " + "[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or " + "WORLD_FRAME coordinates"}, - {"renderImage", pybullet_renderImageObsolete, METH_VARARGS, - "obsolete, please use getCameraImage and getViewProjectionMatrices instead" - }, + {"renderImage", pybullet_renderImageObsolete, METH_VARARGS, + "obsolete, please use getCameraImage and getViewProjectionMatrices instead"}, - { "getCameraImage",(PyCFunction)pybullet_getCameraImage, METH_VARARGS| METH_KEYWORDS, - "Render an image (given the pixel resolution width, height, camera viewMatrix " - ", projectionMatrix, lightDirection, lightColor, lightDistance, shadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff, and renderer), and return the " - "8-8-8bit RGB pixel data and floating point depth values" + {"getCameraImage", (PyCFunction)pybullet_getCameraImage, METH_VARARGS | METH_KEYWORDS, + "Render an image (given the pixel resolution width, height, camera viewMatrix " + ", projectionMatrix, lightDirection, lightColor, lightDistance, shadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff, and renderer), and return the " + "8-8-8bit RGB pixel data and floating point depth values" #ifdef PYBULLET_USE_NUMPY - " as NumPy arrays" + " as NumPy arrays" #endif - }, - - { "computeViewMatrix", (PyCFunction)pybullet_computeViewMatrix, METH_VARARGS | METH_KEYWORDS, - "Compute a camera viewmatrix from camera eye, target position and up vector " - }, - - { "computeViewMatrixFromYawPitchRoll",(PyCFunction)pybullet_computeViewMatrixFromYawPitchRoll, METH_VARARGS | METH_KEYWORDS, - "Compute a camera viewmatrix from camera eye, target position and up vector " - }, - - { "computeProjectionMatrix", (PyCFunction)pybullet_computeProjectionMatrix, METH_VARARGS | METH_KEYWORDS, - "Compute a camera projection matrix from screen left/right/bottom/top/near/far values" }, - { "computeProjectionMatrixFOV", (PyCFunction)pybullet_computeProjectionMatrixFOV, METH_VARARGS | METH_KEYWORDS, - "Compute a camera projection matrix from fov, aspect ratio, near, far values" - }, + {"computeViewMatrix", (PyCFunction)pybullet_computeViewMatrix, METH_VARARGS | METH_KEYWORDS, + "Compute a camera viewmatrix from camera eye, target position and up vector "}, - {"getContactPoints", (PyCFunction)pybullet_getContactPointData, METH_VARARGS | METH_KEYWORDS, - "Return existing contact points after the stepSimulation command. " - "Optional arguments one or two object unique " - "ids, that need to be involved in the contact."}, + {"computeViewMatrixFromYawPitchRoll", (PyCFunction)pybullet_computeViewMatrixFromYawPitchRoll, METH_VARARGS | METH_KEYWORDS, + "Compute a camera viewmatrix from camera eye, target position and up vector "}, - {"getClosestPoints", (PyCFunction)pybullet_getClosestPointData, METH_VARARGS | METH_KEYWORDS, - "Compute the closest points between two objects, if the distance is below a given threshold." - "Input is two objects unique ids and distance threshold." - }, + {"computeProjectionMatrix", (PyCFunction)pybullet_computeProjectionMatrix, METH_VARARGS | METH_KEYWORDS, + "Compute a camera projection matrix from screen left/right/bottom/top/near/far values"}, - { "getOverlappingObjects", (PyCFunction)pybullet_getOverlappingObjects, METH_VARARGS | METH_KEYWORDS, - "Return all the objects that have overlap with a given " - "axis-aligned bounding box volume (AABB)." - "Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]." - }, + {"computeProjectionMatrixFOV", (PyCFunction)pybullet_computeProjectionMatrixFOV, METH_VARARGS | METH_KEYWORDS, + "Compute a camera projection matrix from fov, aspect ratio, near, far values"}, + {"getContactPoints", (PyCFunction)pybullet_getContactPointData, METH_VARARGS | METH_KEYWORDS, + "Return existing contact points after the stepSimulation command. " + "Optional arguments one or two object unique " + "ids, that need to be involved in the contact."}, - { "addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS, + {"getClosestPoints", (PyCFunction)pybullet_getClosestPointData, METH_VARARGS | METH_KEYWORDS, + "Compute the closest points between two objects, if the distance is below a given threshold." + "Input is two objects unique ids and distance threshold."}, + + {"getOverlappingObjects", (PyCFunction)pybullet_getOverlappingObjects, METH_VARARGS | METH_KEYWORDS, + "Return all the objects that have overlap with a given " + "axis-aligned bounding box volume (AABB)." + "Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]."}, + + {"addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS, "Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. " - "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item." - }, + "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."}, - - { "addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS, + {"addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS, "Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds " - "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item." - }, + "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."}, - { "addUserDebugParameter", (PyCFunction)pybullet_addUserDebugParameter, METH_VARARGS | METH_KEYWORDS, - "Add a user debug parameter, such as a slider, that can be controlled using a GUI." - }, - { "readUserDebugParameter", (PyCFunction)pybullet_readUserDebugParameter, METH_VARARGS | METH_KEYWORDS, - "Read the current value of a user debug parameter, given the user debug item unique id." - }, + {"addUserDebugParameter", (PyCFunction)pybullet_addUserDebugParameter, METH_VARARGS | METH_KEYWORDS, + "Add a user debug parameter, such as a slider, that can be controlled using a GUI."}, + {"readUserDebugParameter", (PyCFunction)pybullet_readUserDebugParameter, METH_VARARGS | METH_KEYWORDS, + "Read the current value of a user debug parameter, given the user debug item unique id."}, - { "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS, - "remove a user debug draw item, giving its unique id" - }, + {"removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS, + "remove a user debug draw item, giving its unique id"}, + {"removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS, + "remove all user debug draw items"}, - { "removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS, - "remove all user debug draw items" - }, + {"setDebugObjectColor", (PyCFunction)pybullet_setDebugObjectColor, METH_VARARGS | METH_KEYWORDS, + "Override the wireframe debug drawing color for a particular object unique id / link index." + "If you ommit the color, the custom color will be removed."}, - { "setDebugObjectColor", (PyCFunction)pybullet_setDebugObjectColor, METH_VARARGS | METH_KEYWORDS, - "Override the wireframe debug drawing color for a particular object unique id / link index." - "If you ommit the color, the custom color will be removed." - }, + {"configureDebugVisualizer", (PyCFunction)pybullet_configureDebugVisualizer, METH_VARARGS | METH_KEYWORDS, + "For the 3D OpenGL Visualizer, enable/disable GUI, shadows."}, - { "configureDebugVisualizer", (PyCFunction)pybullet_configureDebugVisualizer, METH_VARARGS | METH_KEYWORDS, - "For the 3D OpenGL Visualizer, enable/disable GUI, shadows." - }, + {"resetDebugVisualizerCamera", (PyCFunction)pybullet_resetDebugVisualizerCamera, METH_VARARGS | METH_KEYWORDS, + "For the 3D OpenGL Visualizer, set the camera distance, yaw, pitch and target position."}, - { "resetDebugVisualizerCamera", (PyCFunction)pybullet_resetDebugVisualizerCamera, METH_VARARGS | METH_KEYWORDS, - "For the 3D OpenGL Visualizer, set the camera distance, yaw, pitch and target position." - }, + {"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS | METH_KEYWORDS, + "Return the visual shape information for one object."}, - + {"resetVisualShapeData", (PyCFunction)pybullet_resetVisualShapeData, METH_VARARGS | METH_KEYWORDS, + "Reset part of the visual shape information for one object."}, - {"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS| METH_KEYWORDS, - "Return the visual shape information for one object." }, - - {"resetVisualShapeData", (PyCFunction)pybullet_resetVisualShapeData, METH_VARARGS| METH_KEYWORDS, - "Reset part of the visual shape information for one object." }, - - {"loadTexture", (PyCFunction)pybullet_loadTexture, METH_VARARGS| METH_KEYWORDS, - "Load texture file." }, - - {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, - "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to " - "quaternion [x,y,z,w]"}, + {"loadTexture", (PyCFunction)pybullet_loadTexture, METH_VARARGS | METH_KEYWORDS, + "Load texture file."}, - {"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS, - "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF " - "convention"}, + {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, + "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to " + "quaternion [x,y,z,w]"}, - {"getMatrixFromQuaternion", pybullet_getMatrixFromQuaternion,METH_VARARGS, - "Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"}, + {"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS, + "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF " + "convention"}, - {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS| METH_KEYWORDS, - "Given an object id, joint positions, joint velocities and joint " - "accelerations, compute the joint forces using Inverse Dynamics"}, - - {"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics, - METH_VARARGS| METH_KEYWORDS, - "Inverse Kinematics bindings: Given an object id, " - "current joint positions and target position" - " for the end effector," - "compute the inverse kinematics and return the new joint state"}, - - { "getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS, - "Get Virtual Reality events, for example to track VR controllers position/buttons" - }, - { "setVRCameraState", (PyCFunction)pybullet_setVRCameraState, METH_VARARGS | METH_KEYWORDS, - "Set properties of the VR Camera such as its root transform " - "for teleporting or to track objects (camera inside a vehicle for example)." - }, - { "getKeyboardEvents", (PyCFunction)pybullet_getKeyboardEvents, METH_VARARGS | METH_KEYWORDS, - "Get Keyboard events, keycode and state (KEY_IS_DOWN, KEY_WAS_TRIGGER, KEY_WAS_RELEASED)" - }, + {"getMatrixFromQuaternion", pybullet_getMatrixFromQuaternion, METH_VARARGS, + "Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"}, - { "startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, - "Start logging of state, such as robot base position, orientation, joint positions etc. " - "Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), " - "fileName, optional objectUniqueId. Function returns int loggingUniqueId" - }, - { "stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, - "Stop logging of robot state, given a loggingUniqueId." - }, - { "rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, - "Cast a ray and return the first object hit, if any. " - "Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates" - }, - { "setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS, - "Set the timeOut in seconds, used for most of the API calls." - }, - // todo(erwincoumans) - // saveSnapshot - // loadSnapshot - // raycast info - // object names + {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS, + "Given an object id, joint positions, joint velocities and joint " + "accelerations, compute the joint forces using Inverse Dynamics"}, - {NULL, NULL, 0, NULL} /* Sentinel */ + {"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics, + METH_VARARGS | METH_KEYWORDS, + "Inverse Kinematics bindings: Given an object id, " + "current joint positions and target position" + " for the end effector," + "compute the inverse kinematics and return the new joint state"}, + + {"getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS, + "Get Virtual Reality events, for example to track VR controllers position/buttons"}, + {"setVRCameraState", (PyCFunction)pybullet_setVRCameraState, METH_VARARGS | METH_KEYWORDS, + "Set properties of the VR Camera such as its root transform " + "for teleporting or to track objects (camera inside a vehicle for example)."}, + {"getKeyboardEvents", (PyCFunction)pybullet_getKeyboardEvents, METH_VARARGS | METH_KEYWORDS, + "Get Keyboard events, keycode and state (KEY_IS_DOWN, KEY_WAS_TRIGGER, KEY_WAS_RELEASED)"}, + + {"startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, + "Start logging of state, such as robot base position, orientation, joint positions etc. " + "Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), " + "fileName, optional objectUniqueId. Function returns int loggingUniqueId"}, + {"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, + "Stop logging of robot state, given a loggingUniqueId."}, + {"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, + "Cast a ray and return the first object hit, if any. " + "Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"}, + {"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS, + "Set the timeOut in seconds, used for most of the API calls."}, + // todo(erwincoumans) + // saveSnapshot + // loadSnapshot + // raycast info + // object names + + {NULL, NULL, 0, NULL} /* Sentinel */ }; - ///copied from CommonWindowInterface.h +///copied from CommonWindowInterface.h - -enum { +enum +{ B3G_ESCAPE = 27, B3G_F1 = 0xff00, B3G_F2, @@ -5300,15 +5309,15 @@ enum { #if PY_MAJOR_VERSION >= 3 static struct PyModuleDef moduledef = { - PyModuleDef_HEAD_INIT, "pybullet", /* m_name */ - "Python bindings for Bullet Physics Robotics API (also known as Shared " - "Memory API)", /* m_doc */ - -1, /* m_size */ - SpamMethods, /* m_methods */ - NULL, /* m_reload */ - NULL, /* m_traverse */ - NULL, /* m_clear */ - NULL, /* m_free */ + PyModuleDef_HEAD_INIT, "pybullet", /* m_name */ + "Python bindings for Bullet Physics Robotics API (also known as Shared " + "Memory API)", /* m_doc */ + -1, /* m_size */ + SpamMethods, /* m_methods */ + NULL, /* m_reload */ + NULL, /* m_traverse */ + NULL, /* m_clear */ + NULL, /* m_free */ }; #endif @@ -5319,115 +5328,110 @@ PyInit_pybullet(void) initpybullet(void) #endif { - - PyObject* m; + PyObject* m; #if PY_MAJOR_VERSION >= 3 - m = PyModule_Create(&moduledef); + m = PyModule_Create(&moduledef); #else - m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet"); + m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet"); #endif #if PY_MAJOR_VERSION >= 3 - if (m == NULL) return m; + if (m == NULL) return m; #else - if (m == NULL) return; + if (m == NULL) return; #endif - 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 - PyModule_AddIntConstant(m, "UDP", eCONNECT_UDP); // user read - PyModule_AddIntConstant(m, "TCP", eCONNECT_TCP); // user read + 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 + PyModule_AddIntConstant(m, "UDP", eCONNECT_UDP); // user read + PyModule_AddIntConstant(m, "TCP", eCONNECT_TCP); // user read - PyModule_AddIntConstant(m, "JOINT_REVOLUTE", eRevoluteType); // user read - PyModule_AddIntConstant(m, "JOINT_PRISMATIC", ePrismaticType); // user read - PyModule_AddIntConstant(m, "JOINT_SPHERICAL", eSphericalType); // user read - PyModule_AddIntConstant(m, "JOINT_PLANAR", ePlanarType); // user read - PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read - PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read + PyModule_AddIntConstant(m, "JOINT_REVOLUTE", eRevoluteType); // user read + PyModule_AddIntConstant(m, "JOINT_PRISMATIC", ePrismaticType); // user read + PyModule_AddIntConstant(m, "JOINT_SPHERICAL", eSphericalType); // user read + PyModule_AddIntConstant(m, "JOINT_PLANAR", ePlanarType); // user read + PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read + PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read - PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read + PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read + PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); + PyModule_AddIntConstant(m, "VELOCITY_CONTROL", + CONTROL_MODE_VELOCITY); // user read + PyModule_AddIntConstant(m, "POSITION_CONTROL", + CONTROL_MODE_POSITION_VELOCITY_PD); // user read - PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); - PyModule_AddIntConstant(m, "VELOCITY_CONTROL", - CONTROL_MODE_VELOCITY); // user read - PyModule_AddIntConstant(m, "POSITION_CONTROL", - CONTROL_MODE_POSITION_VELOCITY_PD); // user read + PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME); + PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME); - PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME); - PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME); + PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS); + PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS); - PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS); - PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS); + PyModule_AddIntConstant(m, "VR_BUTTON_IS_DOWN", eButtonIsDown); + PyModule_AddIntConstant(m, "VR_BUTTON_WAS_TRIGGERED", eButtonTriggered); + PyModule_AddIntConstant(m, "VR_BUTTON_WAS_RELEASED", eButtonReleased); - PyModule_AddIntConstant(m, "VR_BUTTON_IS_DOWN", eButtonIsDown); - PyModule_AddIntConstant(m, "VR_BUTTON_WAS_TRIGGERED", eButtonTriggered); - PyModule_AddIntConstant(m, "VR_BUTTON_WAS_RELEASED", eButtonReleased); - - PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS); - PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS); + PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS); + PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS); - PyModule_AddIntConstant(m, "KEY_IS_DOWN", eButtonIsDown); - PyModule_AddIntConstant(m, "KEY_WAS_TRIGGERED", eButtonTriggered); - PyModule_AddIntConstant(m, "KEY_WAS_RELEASED", eButtonReleased); + PyModule_AddIntConstant(m, "KEY_IS_DOWN", eButtonIsDown); + PyModule_AddIntConstant(m, "KEY_WAS_TRIGGERED", eButtonTriggered); + PyModule_AddIntConstant(m, "KEY_WAS_RELEASED", eButtonReleased); - PyModule_AddIntConstant(m, "STATE_LOGGING_MINITAUR", STATE_LOGGING_MINITAUR); - PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT); - PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS); + PyModule_AddIntConstant(m, "STATE_LOGGING_MINITAUR", STATE_LOGGING_MINITAUR); + PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT); + PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS); - PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI); - PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS); - PyModule_AddIntConstant(m, "COV_ENABLE_WIREFRAME", COV_ENABLE_WIREFRAME); + PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI); + PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS); + PyModule_AddIntConstant(m, "COV_ENABLE_WIREFRAME", COV_ENABLE_WIREFRAME); - PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER); - PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL); + PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER); + PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL); - PyModule_AddIntConstant(m,"B3G_F1",B3G_F1); - PyModule_AddIntConstant(m,"B3G_F2",B3G_F2); - PyModule_AddIntConstant(m,"B3G_F3",B3G_F3); - PyModule_AddIntConstant(m,"B3G_F4",B3G_F4); - PyModule_AddIntConstant(m,"B3G_F5",B3G_F5); - PyModule_AddIntConstant(m,"B3G_F6",B3G_F6); - PyModule_AddIntConstant(m,"B3G_F7",B3G_F7); - PyModule_AddIntConstant(m,"B3G_F8",B3G_F8); - PyModule_AddIntConstant(m,"B3G_F9",B3G_F9); - PyModule_AddIntConstant(m,"B3G_F10",B3G_F10); - PyModule_AddIntConstant(m,"B3G_F11",B3G_F11); - PyModule_AddIntConstant(m,"B3G_F12",B3G_F12); - PyModule_AddIntConstant(m,"B3G_F13",B3G_F13); - PyModule_AddIntConstant(m,"B3G_F14",B3G_F14); - PyModule_AddIntConstant(m,"B3G_F15",B3G_F15); - PyModule_AddIntConstant(m,"B3G_LEFT_ARROW",B3G_LEFT_ARROW); - PyModule_AddIntConstant(m,"B3G_RIGHT_ARROW",B3G_RIGHT_ARROW); - PyModule_AddIntConstant(m,"B3G_UP_ARROW",B3G_UP_ARROW); - PyModule_AddIntConstant(m,"B3G_DOWN_ARROW",B3G_DOWN_ARROW); - PyModule_AddIntConstant(m,"B3G_PAGE_UP",B3G_PAGE_UP); - PyModule_AddIntConstant(m,"B3G_PAGE_DOWN",B3G_PAGE_DOWN); - PyModule_AddIntConstant(m,"B3G_END",B3G_END); - PyModule_AddIntConstant(m,"B3G_HOME",B3G_HOME); - PyModule_AddIntConstant(m,"B3G_INSERT",B3G_INSERT); - PyModule_AddIntConstant(m,"B3G_DELETE",B3G_DELETE); - PyModule_AddIntConstant(m,"B3G_BACKSPACE", B3G_BACKSPACE); - PyModule_AddIntConstant(m,"B3G_SHIFT", B3G_SHIFT); - PyModule_AddIntConstant(m,"B3G_CONTROL", B3G_CONTROL); - PyModule_AddIntConstant(m,"B3G_ALT", B3G_ALT); - PyModule_AddIntConstant(m,"B3G_RETURN", B3G_RETURN); + PyModule_AddIntConstant(m, "B3G_F1", B3G_F1); + PyModule_AddIntConstant(m, "B3G_F2", B3G_F2); + PyModule_AddIntConstant(m, "B3G_F3", B3G_F3); + PyModule_AddIntConstant(m, "B3G_F4", B3G_F4); + PyModule_AddIntConstant(m, "B3G_F5", B3G_F5); + PyModule_AddIntConstant(m, "B3G_F6", B3G_F6); + PyModule_AddIntConstant(m, "B3G_F7", B3G_F7); + PyModule_AddIntConstant(m, "B3G_F8", B3G_F8); + PyModule_AddIntConstant(m, "B3G_F9", B3G_F9); + PyModule_AddIntConstant(m, "B3G_F10", B3G_F10); + PyModule_AddIntConstant(m, "B3G_F11", B3G_F11); + PyModule_AddIntConstant(m, "B3G_F12", B3G_F12); + PyModule_AddIntConstant(m, "B3G_F13", B3G_F13); + PyModule_AddIntConstant(m, "B3G_F14", B3G_F14); + PyModule_AddIntConstant(m, "B3G_F15", B3G_F15); + PyModule_AddIntConstant(m, "B3G_LEFT_ARROW", B3G_LEFT_ARROW); + PyModule_AddIntConstant(m, "B3G_RIGHT_ARROW", B3G_RIGHT_ARROW); + PyModule_AddIntConstant(m, "B3G_UP_ARROW", B3G_UP_ARROW); + PyModule_AddIntConstant(m, "B3G_DOWN_ARROW", B3G_DOWN_ARROW); + PyModule_AddIntConstant(m, "B3G_PAGE_UP", B3G_PAGE_UP); + PyModule_AddIntConstant(m, "B3G_PAGE_DOWN", B3G_PAGE_DOWN); + PyModule_AddIntConstant(m, "B3G_END", B3G_END); + PyModule_AddIntConstant(m, "B3G_HOME", B3G_HOME); + PyModule_AddIntConstant(m, "B3G_INSERT", B3G_INSERT); + PyModule_AddIntConstant(m, "B3G_DELETE", B3G_DELETE); + PyModule_AddIntConstant(m, "B3G_BACKSPACE", B3G_BACKSPACE); + PyModule_AddIntConstant(m, "B3G_SHIFT", B3G_SHIFT); + PyModule_AddIntConstant(m, "B3G_CONTROL", B3G_CONTROL); + PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT); + PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN); - - - SpamError = PyErr_NewException("pybullet.error", NULL, NULL); - Py_INCREF(SpamError); - PyModule_AddObject(m, "error", SpamError); + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); + Py_INCREF(SpamError); + PyModule_AddObject(m, "error", SpamError); #ifdef PYBULLET_USE_NUMPY - // Initialize numpy array. - import_array(); -#endif //PYBULLET_USE_NUMPY - + // Initialize numpy array. + import_array(); +#endif //PYBULLET_USE_NUMPY #if PY_MAJOR_VERSION >= 3 - return m; + return m; #endif }