From 9d2d286c46ebacfd97259b57ced6c13b1de9bdfe Mon Sep 17 00:00:00 2001 From: Logan Su Date: Thu, 1 Sep 2016 18:12:14 -0700 Subject: [PATCH 1/6] Fix joint orientations when loading URDF files. --- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 20 ++++++------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index c3aaee49b..85ee651f0 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -286,6 +286,7 @@ void ConvertURDF2BulletInternal( btTransform offsetInA,offsetInB; offsetInA = parentLocalInertialFrame.inverse()*parent2joint; offsetInB = localInertialFrame.inverse(); + btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation(); bool disableParentCollision = true; switch (jointType) @@ -295,14 +296,9 @@ void ConvertURDF2BulletInternal( if (createMultiBody) { //todo: adjust the center of mass transform and pivot axis properly - - //b3Printf("Fixed joint (btMultiBody)\n"); - btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation(); cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, - rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin()); + parentRotToThis, offsetInA.getOrigin(),-offsetInB.getOrigin()); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); - - } else { //b3Printf("Fixed joint\n"); @@ -319,14 +315,12 @@ void ConvertURDF2BulletInternal( { if (createMultiBody) { - - cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, - offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), + parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); - cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping; - cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction; + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping; + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction; creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); } else @@ -344,12 +338,10 @@ void ConvertURDF2BulletInternal( { if (createMultiBody) { - cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, - offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), + parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(), -offsetInB.getOrigin(), disableParentCollision); - creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit); world1->addMultiBodyConstraint(con); From d5ec5ca9a0e4a02b51e7f5e4971d05d89c33fae0 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 6 Sep 2016 13:07:06 -0700 Subject: [PATCH 2/6] Update IDConfig.hpp --- src/BulletInverseDynamics/IDConfig.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp index 8e6577912..c34f98941 100644 --- a/src/BulletInverseDynamics/IDConfig.hpp +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -31,6 +31,10 @@ #include "IDErrorMessages.hpp" #ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H +/* +#include "IDConfigEigen.hpp" +#include "IDConfigBuiltin.hpp" +*/ #define INVDYN_INCLUDE_HELPER_2(x) #x #define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x) #include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H) From 4944aca28bca28bf2650dfebece6e5e926b88a15 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 6 Sep 2016 13:26:08 -0700 Subject: [PATCH 3/6] Update our_gl.cpp --- examples/TinyRenderer/our_gl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp index 639db9b0b..eb5a05a78 100644 --- a/examples/TinyRenderer/our_gl.cpp +++ b/examples/TinyRenderer/our_gl.cpp @@ -59,7 +59,7 @@ Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) { return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator } -void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix, int objectIndex) +void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix) { triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0); } From edef18e161718a404781677a3630bec26fcb37d2 Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Tue, 6 Sep 2016 23:27:34 -0700 Subject: [PATCH 4/6] [python] Convert physics calls to double precision. In order to feed Bullet the correct values when compiled with double precision the pybullet interface needs to pass double precision values. --- examples/pybullet/pybullet.c | 302 +++++++++++++++++------------------ 1 file changed, 151 insertions(+), 151 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8bc52cfbe..3b87ea258 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -14,7 +14,7 @@ #if PY_MAJOR_VERSION >= 3 #define PyInt_FromLong PyLong_FromLong #define PyString_FromString PyBytes_FromString -#endif +#endif enum eCONNECT_METHOD { @@ -46,7 +46,7 @@ pybullet_stepSimulation(PyObject *self, PyObject *args) { statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm)); statusType = b3GetStatusType(statusHandle); - } + } } Py_INCREF(Py_None); @@ -61,7 +61,7 @@ pybullet_connectPhysicsServer(PyObject *self, PyObject *args) PyErr_SetString(SpamError, "Already connected to physics server, disconnect first."); return NULL; } - + { int method=eCONNECT_GUI; if (!PyArg_ParseTuple(args, "i", &method)) @@ -102,10 +102,10 @@ pybullet_connectPhysicsServer(PyObject *self, PyObject *args) return NULL; } }; - - + + } - + Py_INCREF(Py_None); return Py_None; } @@ -122,7 +122,7 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) b3DisconnectSharedMemory(sm); sm = 0; } - + Py_INCREF(Py_None); return Py_None; } @@ -135,19 +135,19 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) static PyObject * pybullet_loadURDF(PyObject* self, PyObject* args) { - + int size= PySequence_Size(args); - + int bodyIndex = -1; const char* urdfFileName=""; - - float startPosX =0; - float startPosY =0; - float startPosZ = 0; - float startOrnX = 0; - float startOrnY = 0; - float startOrnZ = 0; - float startOrnW = 1; + + 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; if (0==sm) { @@ -157,26 +157,26 @@ pybullet_loadURDF(PyObject* self, PyObject* args) if (size==1) { if (!PyArg_ParseTuple(args, "s", &urdfFileName)) - return NULL; + return NULL; } if (size == 4) { - if (!PyArg_ParseTuple(args, "sfff", &urdfFileName, + if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX,&startPosY,&startPosZ)) return NULL; } if (size==8) { - if (!PyArg_ParseTuple(args, "sfffffff", &urdfFileName, + if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX,&startPosY,&startPosZ, &startOrnX,&startOrnY,&startOrnZ, &startOrnW)) 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); @@ -200,11 +200,11 @@ pybullet_loadURDF(PyObject* self, PyObject* args) return PyLong_FromLong(bodyIndex); } -static float pybullet_internalGetFloatFromSequence(PyObject* seq, int index) +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { - float v = 0.f; + double v = 0.f; PyObject* item; - + if (PyList_Check(seq)) { item = PyList_GET_ITEM(seq, index); @@ -264,7 +264,7 @@ pybullet_loadSDF(PyObject* self, PyObject* args) pylist = PyTuple_New(numBodies); - if (numBodies >0 && numBodies <= MAX_SDF_BODIES) + if (numBodies >0 && numBodies <= MAX_SDF_BODIES) { for (i=0;i= numJoints) || (jointIndex < 0)) { PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; } - + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); @@ -809,10 +809,10 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - + + size= PySequence_Size(args); - + if (size==3) { int bodyIndex; @@ -820,12 +820,12 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) PyObject* ornObj; double pos[3]; double orn[4];//as a quaternion - + if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) { b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; - + { PyObject* seq; int len,i; @@ -865,9 +865,9 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) } Py_DECREF(seq); } - + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - + b3CreatePoseCommandSetBasePosition( commandHandle, pos[0],pos[1],pos[2]); b3CreatePoseCommandSetBaseOrientation( commandHandle, orn[0],orn[1],orn[2],orn[3]); @@ -875,7 +875,7 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) Py_INCREF(Py_None); return Py_None; } - + } PyErr_SetString(SpamError, "error in resetJointState."); return NULL; @@ -892,21 +892,21 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) // index, name, type, q-index, u-index, // flags, joint damping, joint friction // -// The format of the returned list is +// The format of the returned list is // [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 *pyListJointInfo; - + PyObject *pyListJointInfo; + struct b3JointInfo info; - + int bodyIndex = -1; int jointIndex = -1; int jointInfoSize = 8; //size of struct b3JointInfo - + int size= PySequence_Size(args); if (0==sm) @@ -921,7 +921,7 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) { if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { - + // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); pyListJointInfo = PyTuple_New(jointInfoSize); @@ -931,8 +931,8 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // 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_jointName, + // info.m_jointType, // info.m_qIndex, // info.m_uIndex); // printf(" flags=%d jointDamping=%f jointFriction=%f\n", @@ -964,7 +964,7 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) } } } - + Py_INCREF(Py_None); return Py_None; } @@ -980,26 +980,26 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // position, velocity, force torque (6 values), and motor torque // The returned pylist is an array of [float, float, float[6], float] -// TODO(hellojas): check accuracy of position and velocity +// TODO(hellojas): check accuracy of position and velocity // TODO(hellojas): check force torque values static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) { - PyObject *pyListJointForceTorque; - PyObject *pyListJointState; - PyObject *item; - + PyObject *pyListJointForceTorque; + PyObject *pyListJointState; + PyObject *item; + struct b3JointInfo info; struct b3JointSensorState sensorState; - + int bodyIndex = -1; int jointIndex = -1; int sensorStateSize = 4; // size of struct b3JointSensorState int forceTorqueSize = 6; // size of force torque list from b3JointSensorState int i, j; - - + + int size= PySequence_Size(args); if (0==sm) @@ -1017,7 +1017,7 @@ pybullet_getJointState(PyObject* self, PyObject* args) b3RequestActualStateCommandInit(sm, bodyIndex); b3SharedMemoryStatusHandle status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - + status_type = b3GetStatusType(status_handle); if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { @@ -1028,25 +1028,25 @@ pybullet_getJointState(PyObject* self, PyObject* args) pyListJointState = PyTuple_New(sensorStateSize); pyListJointForceTorque = PyTuple_New(forceTorqueSize); - + b3GetJointState(sm, status_handle, jointIndex, &sensorState); - - PyTuple_SetItem(pyListJointState, 0, + + PyTuple_SetItem(pyListJointState, 0, PyFloat_FromDouble(sensorState.m_jointPosition)); - PyTuple_SetItem(pyListJointState, 1, + 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])); } - - PyTuple_SetItem(pyListJointState, 2, + + PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); - - PyTuple_SetItem(pyListJointState, 3, + + PyTuple_SetItem(pyListJointState, 3, PyFloat_FromDouble(sensorState.m_jointMotorTorque)); - + return pyListJointState; } } else @@ -1054,7 +1054,7 @@ pybullet_getJointState(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "getJointState expects 2 arguments (objectUniqueId and joint index)."); return NULL; } - + Py_INCREF(Py_None); return Py_None; } @@ -1092,7 +1092,7 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) // a view and projection matrix in renderImage() // // // Args: -// matrix - float[16] which will be set by values from objMat +// vector - float[3] which will be set by values from objMat static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) { int i, len; @@ -1256,7 +1256,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) int size= PySequence_Size(args); float viewMatrix[16]; float projectionMatrix[16]; - + float cameraPos[3]; float targetPos[3]; float cameraUp[3]; @@ -1267,7 +1267,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) // inialize cmd b3SharedMemoryCommandHandle command; - + if (0==sm) { @@ -1353,16 +1353,16 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { 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); - + b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,roll,upAxisIndex); aspect = width/height; b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal); @@ -1374,10 +1374,10 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { PyErr_SetString(SpamError, "Error parsing arguments"); } - - - - + + + + } else { @@ -1389,9 +1389,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { b3SharedMemoryStatusHandle statusHandle; int statusType; - + //b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); - + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType==CMD_CAMERA_IMAGE_COMPLETED) @@ -1401,7 +1401,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) PyObject *pylistRGB; PyObject* pylistDep; PyObject* pylistSeg; - + int i, j, p; b3GetCameraImageData(sm, &imageData); @@ -1433,8 +1433,8 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) item2 = PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]); PyTuple_SetItem(pylistSeg, depIndex, item2); } - - + + for (p=0; p 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; - + double quat[4]; PyObject* quatObj; - + if (PyArg_ParseTuple(args, "O", &quatObj)) { PyObject* seq; @@ -1727,7 +1727,7 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) return Py_None; } -///Given an object id, joint positions, joint velocities and joint accelerations, +///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) { @@ -1797,7 +1797,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg jointForcesOutput); { { - + int i; pylist = PyTuple_New(dofCount); for (i = 0; i Date: Tue, 6 Sep 2016 23:43:57 -0700 Subject: [PATCH 5/6] [format] Convert pybullet.c to consistent style. --- examples/pybullet/pybullet.c | 3042 ++++++++++++++++------------------ 1 file changed, 1438 insertions(+), 1604 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 3b87ea258..161ecf982 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2,129 +2,109 @@ #include "../SharedMemory/PhysicsDirectC_API.h" #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" - - #ifdef __APPLE__ #include #else #include #endif - #if PY_MAJOR_VERSION >= 3 #define PyInt_FromLong PyLong_FromLong #define PyString_FromString PyBytes_FromString #endif -enum eCONNECT_METHOD -{ - eCONNECT_GUI=1, - eCONNECT_DIRECT=2, - eCONNECT_SHARED_MEMORY=3, +enum eCONNECT_METHOD { + eCONNECT_GUI = 1, + eCONNECT_DIRECT = 2, + eCONNECT_SHARED_MEMORY = 3, }; - - -static PyObject *SpamError; -static b3PhysicsClientHandle sm=0; +static PyObject* SpamError; +static b3PhysicsClientHandle sm = 0; // Step through one timestep of the simulation -static PyObject * -pybullet_stepSimulation(PyObject *self, PyObject *args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; +static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) { + if (0 == sm) { + 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; - - if (b3CanSubmitCommand(sm)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm)); - statusType = b3GetStatusType(statusHandle); - } - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject * -pybullet_connectPhysicsServer(PyObject *self, PyObject *args) -{ - if (0!=sm) - { - PyErr_SetString(SpamError, "Already connected to physics server, disconnect first."); - return NULL; +static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) { + if (0 != sm) { + PyErr_SetString(SpamError, + "Already connected to physics server, disconnect first."); + return NULL; + } + + { + int method = eCONNECT_GUI; + if (!PyArg_ParseTuple(args, "i", &method)) { + PyErr_SetString(SpamError, + "connectPhysicsServer expected argument eCONNECT_GUI, " + "eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY"); + return NULL; } - { - int method=eCONNECT_GUI; - if (!PyArg_ParseTuple(args, "i", &method)) - { - PyErr_SetString(SpamError, "connectPhysicsServer expected argument eCONNECT_GUI, eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY"); - return NULL; - } - - switch (method) - { - case eCONNECT_GUI: - { - int argc=0; - char* argv[1]={0}; + 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(SHARED_MEMORY_KEY); - break; - } + break; + } + case eCONNECT_DIRECT: { + sm = b3ConnectPhysicsDirect(); + break; + } + case eCONNECT_SHARED_MEMORY: { + sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); + break; + } + default: { + PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument"); + return NULL; + } + }; + } - default: - { - PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument"); - return NULL; - } - }; - - - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject * -pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - { - b3DisconnectSharedMemory(sm); - sm = 0; - } +static PyObject* pybullet_disconnectPhysicsServer(PyObject* self, + PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + b3DisconnectSharedMemory(sm); + sm = 0; + } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } // Load a URDF file indicating the links and joints of an object @@ -132,755 +112,657 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) // 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) -{ +static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) { + int size = PySequence_Size(args); - int size= PySequence_Size(args); + 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; - 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; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (size == 1) { + if (!PyArg_ParseTuple(args, "s", &urdfFileName)) return NULL; + } + if (size == 4) { + if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX, &startPosY, + &startPosZ)) + return NULL; + } + if (size == 8) { + if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX, + &startPosY, &startPosZ, &startOrnX, &startOrnY, + &startOrnZ, &startOrnW)) + return NULL; + } - if (0==sm) - { - 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); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_URDF_LOADING_COMPLETED) { + PyErr_SetString(SpamError, "Cannot load URDF file."); + return NULL; } - if (size==1) - { - if (!PyArg_ParseTuple(args, "s", &urdfFileName)) - return NULL; - } - if (size == 4) - { - if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, - &startPosX,&startPosY,&startPosZ)) - return NULL; - } - if (size==8) - { - if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, - &startPosX,&startPosY,&startPosZ, - &startOrnX,&startOrnY,&startOrnZ, &startOrnW)) - 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 ); - 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); + bodyIndex = b3GetStatusBodyIndex(statusHandle); + } else { + PyErr_SetString(SpamError, + "Empty filename, method expects 1, 4 or 8 arguments."); + return NULL; + } + return PyLong_FromLong(bodyIndex); } -static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) -{ - double v = 0.f; - PyObject* item; +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { + double v = 0.0; + PyObject* item; - if (PyList_Check(seq)) - { - item = PyList_GET_ITEM(seq, index); - v = PyFloat_AsDouble(item); - } - else - { - item = PyTuple_GET_ITEM(seq,index); - v = PyFloat_AsDouble(item); - } - return v; + if (PyList_Check(seq)) { + item = PyList_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } else { + item = PyTuple_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + return v; } - #define MAX_SDF_BODIES 512 -static PyObject* -pybullet_loadSDF(PyObject* self, PyObject* args) -{ - const char* sdfFileName=""; - int size= PySequence_Size(args); - int numBodies = 0; - int i; - int bodyIndicesOut[MAX_SDF_BODIES]; - PyObject* pylist=0; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle; +static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) { + const char* sdfFileName = ""; + int size = PySequence_Size(args); + int numBodies = 0; + int i; + int bodyIndicesOut[MAX_SDF_BODIES]; + PyObject* pylist = 0; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - if (size==1) - { - if (!PyArg_ParseTuple(args, "s", &sdfFileName)) - return NULL; - } + if (size == 1) { + if (!PyArg_ParseTuple(args, "s", &sdfFileName)) 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 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) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm)); - } - Py_INCREF(Py_None); - return Py_None; -} - -static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) -{ - int size; - int bodyIndex, jointIndex, controlMode; - - double targetPosition=0; - double targetVelocity=0; - double maxForce=100000; - double appliedForce = 0; - double kp=0.1; - double kd=1.0; - int valid = 0; - - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - size= PySequence_Size(args); - if (size==4) - { - double targetValue = 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; - //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; - double targetValue = 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; - } - - - - 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; - } - - 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); - - 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; - } - - 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: - { - } - }; - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - - Py_INCREF(Py_None); - return Py_None; - } - PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); +static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; + } + + { + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + sm, b3InitResetSimulationCommand(sm)); + } + Py_INCREF(Py_None); + return Py_None; } -static PyObject * -pybullet_setRealTimeSimulation(PyObject* self, PyObject* args) -{ - if (0 == sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { + int size; + int bodyIndex, jointIndex, controlMode; - { - int enableRealTimeSimulation = 0; - int ret; + 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; - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) - { - PyErr_SetString(SpamError, "setRealTimeSimulation expected a single value (integer)."); - return NULL; - } - ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + 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; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } + 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; - Py_INCREF(Py_None); - return Py_None; -} + 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; - - -// Set the gravity of the world with (x, y, z) arguments -static PyObject * -pybullet_setGravity(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; } - { - double gravX=0; - double gravY=0; - double gravZ=-10; - int ret; - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; - - if (!PyArg_ParseTuple(args, "ddd", &gravX,&gravY,&gravZ)) - { - PyErr_SetString(SpamError, "setGravity expected (x,y,z) values."); - return NULL; - } - ret = b3PhysicsParamSetGravity(command, gravX,gravY, gravZ); - //ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } - - Py_INCREF(Py_None); - return Py_None; -} - -static PyObject * -pybullet_setTimeStep(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - 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; } - { - double timeStep=0.001; - int ret; + commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - if (!PyArg_ParseTuple(args, "d", &timeStep)) - { - PyErr_SetString(SpamError, "setTimeStep expected a single value (double)."); - return NULL; - } - ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } + 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_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); 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) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + int enableRealTimeSimulation = 0; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) { + PyErr_SetString( + SpamError, + "setRealTimeSimulation expected a single value (integer)."); + return NULL; + } + ret = + b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + 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) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double gravX = 0.0; + double gravY = 0.0; + double gravZ = -10.0; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "ddd", &gravX, &gravY, &gravZ)) { + PyErr_SetString(SpamError, "setGravity expected (x,y,z) values."); + return NULL; + } + ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ); + // ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} + +static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double timeStep = 0.001; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "d", &timeStep)) { + PyErr_SetString(SpamError, + "setTimeStep expected a single value (double)."); + return NULL; + } + ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} // 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[3]) -{ - basePosition[0] = 0.; - basePosition[1] = 0.; - basePosition[2] = 0.; +static int pybullet_internalGetBasePositionAndOrientation( + int bodyIndex, double basePosition[3], double baseOrientation[3]) { + 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.; - { + { + { + 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[]; + int i; - const int status_type = b3GetStatusType(status_handle); - const double* actualStateQ; - // const double* jointReactionForces[]; - int i; + 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 // values for the base link of your object // 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) -{ - int bodyIndex = -1; - double basePosition[3]; - double baseOrientation[4]; - PyObject *pylistPos; - PyObject *pylistOrientation; +static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, + PyObject* args) { + int bodyIndex = -1; + double basePosition[3]; + double baseOrientation[4]; + PyObject* pylistPos; + PyObject* pylistOrientation; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { + PyErr_SetString(SpamError, "Expected a body index (integer)."); + return NULL; + } + + if (0 == pybullet_internalGetBasePositionAndOrientation( + bodyIndex, basePosition, baseOrientation)) { + PyErr_SetString(SpamError, + "GetBasePositionAndOrientation failed (#joints/links " + "exceeds maximum?)."); + 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 (!PyArg_ParseTuple(args, "i", &bodyIndex )) - { - PyErr_SetString(SpamError, "Expected a body index (integer)."); - return NULL; - } - - if (0==pybullet_internalGetBasePositionAndOrientation(bodyIndex, basePosition, baseOrientation)) - { - PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed (#joints/links exceeds maximum?)."); - 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); - } - - } - - { - - 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; + { + 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; + } } // 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) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args) { + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - { - int bodyIndex = -1; - int numJoints=0; - if (!PyArg_ParseTuple(args, "i", &bodyIndex )) - { - PyErr_SetString(SpamError, "Expected a body index (integer)."); - return NULL; - } - numJoints = b3GetNumJoints(sm,bodyIndex); + { + int bodyIndex = -1; + int numJoints = 0; + if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { + PyErr_SetString(SpamError, "Expected a body index (integer)."); + return NULL; + } + numJoints = b3GetNumJoints(sm, bodyIndex); #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(numJoints); + 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) -{ - int size; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - - size= PySequence_Size(args); - - if (size==3) - { - int bodyIndex; - int jointIndex; - double targetValue; - - if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) - { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int numJoints; - - numJoints = b3GetNumJoints(sm,bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) - { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } - - commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - - b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - - } - PyErr_SetString(SpamError, "error in resetJointState."); +static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) { + int size; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); 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) -{ - int size; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); + size = PySequence_Size(args); + + if (size == 3) { + int bodyIndex; + int jointIndex; + double targetValue; + + if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int numJoints; + + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) { + PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, + targetValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; } - - - size= PySequence_Size(args); - - if (size==3) - { - int bodyIndex; - PyObject* posObj; - PyObject* ornObj; - double pos[3]; - double orn[4];//as a quaternion - - if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) - { - 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, bodyIndex); - - 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; } +// 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) { + int size; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + size = PySequence_Size(args); + + if (size == 3) { + int bodyIndex; + PyObject* posObj; + PyObject* ornObj; + double pos[3]; + double orn[4]; // as a quaternion + + if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) { + 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, bodyIndex); + + 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; +} // Get the a single joint info for a specific bodyIndex // @@ -896,80 +778,63 @@ pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args) // [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 *pyListJointInfo; +static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) { + PyObject* pyListJointInfo; struct b3JointInfo info; int bodyIndex = -1; int jointIndex = -1; - int jointInfoSize = 8; //size of struct b3JointInfo + int jointInfoSize = 8; // size of struct b3JointInfo - int size= PySequence_Size(args); + int size = PySequence_Size(args); - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - - - if (size==2) // get body index and joint index + if (size == 2) // get body index and joint index { - if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) - { - + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); - pyListJointInfo = PyTuple_New(jointInfoSize); + pyListJointInfo = PyTuple_New(jointInfoSize); - if (b3GetJointInfo(sm, bodyIndex, 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, bodyIndex, 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 // // Args: @@ -983,108 +848,96 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // TODO(hellojas): check accuracy of position and velocity // TODO(hellojas): check force torque values -static PyObject* -pybullet_getJointState(PyObject* self, PyObject* args) -{ - PyObject *pyListJointForceTorque; - PyObject *pyListJointState; - PyObject *item; +static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) { + PyObject* pyListJointForceTorque; + PyObject* pyListJointState; + PyObject* item; struct b3JointInfo info; struct b3JointSensorState sensorState; int bodyIndex = -1; int jointIndex = -1; - int sensorStateSize = 4; // size of struct b3JointSensorState - int forceTorqueSize = 6; // size of force torque list from b3JointSensorState + int sensorStateSize = 4; // size of struct b3JointSensorState + int forceTorqueSize = 6; // size of force torque list from b3JointSensorState int i, j; + int size = PySequence_Size(args); - int size= PySequence_Size(args); - - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (size==2) // get body index and joint index - { - if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) - { - int status_type = 0; - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - - status_type = b3GetStatusType(status_handle); - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); - return NULL; - } - - pyListJointState = PyTuple_New(sensorStateSize); - pyListJointForceTorque = PyTuple_New(forceTorqueSize); - - - 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])); - } - - PyTuple_SetItem(pyListJointState, 2, - pyListJointForceTorque); - - PyTuple_SetItem(pyListJointState, 3, - PyFloat_FromDouble(sensorState.m_jointMotorTorque)); - - return pyListJointState; - } - } else - { - PyErr_SetString(SpamError, "getJointState expects 2 arguments (objectUniqueId and joint index)."); - return NULL; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; } - Py_INCREF(Py_None); - return Py_None; + if (size == 2) // get body index and joint index + { + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return NULL; + } + + pyListJointState = PyTuple_New(sensorStateSize); + pyListJointForceTorque = PyTuple_New(forceTorqueSize); + + 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])); + } + + PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); + + PyTuple_SetItem(pyListJointState, 3, + PyFloat_FromDouble(sensorState.m_jointMotorTorque)); + + return pyListJointState; + } + } else { + PyErr_SetString( + SpamError, + "getJointState expects 2 arguments (objectUniqueId and joint index)."); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; } - - // 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]) -{ - int i, len; - PyObject* seq; +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { + int i, len; + PyObject* seq; - seq = PySequence_Fast(objMat, "expected a sequence"); - len = PySequence_Size(objMat); - if (len==16) - { - for (i = 0; i < len; i++) - { - matrix[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - Py_DECREF(seq); - return 1; + seq = PySequence_Fast(objMat, "expected a sequence"); + len = PySequence_Size(objMat); + if (len == 16) { + for (i = 0; i < len; i++) { + matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); - return 0; + return 1; + } + Py_DECREF(seq); + return 0; } // internal function to set a float vector[3] @@ -1093,136 +946,139 @@ 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* objMat, float vector[3]) -{ - int i, len; - PyObject* seq; +static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) { + int i, len; + PyObject* seq; - seq = PySequence_Fast(objMat, "expected a sequence"); - len = PySequence_Size(objMat); - if (len==3) - { - for (i = 0; i < len; i++) - { - vector[i] = pybullet_internalGetFloatFromSequence(seq,i); - } - Py_DECREF(seq); - return 1; + seq = PySequence_Fast(objMat, "expected a sequence"); + len = PySequence_Size(objMat); + if (len == 3) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); - return 0; + return 1; + } + Py_DECREF(seq); + return 0; } +static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { + int size = PySequence_Size(args); + int objectUniqueIdA = -1; + int objectUniqueIdB = -1; + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + PyObject* pyResultList = 0; -static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) -{ - int size= PySequence_Size(args); - int objectUniqueIdA = -1; - int objectUniqueIdB = -1; - b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - - PyObject* pyResultList=0; - - if (size==1) - { - if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) - { - PyErr_SetString(SpamError, "Error parsing object unique id"); - return NULL; - } + if (size == 1) { + if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) { + PyErr_SetString(SpamError, "Error parsing object unique id"); + return NULL; } - if (size==2) - { - if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA,&objectUniqueIdB)) - { - PyErr_SetString(SpamError, "Error parsing object unique id"); - return NULL; - } + } + if (size == 2) { + if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA, &objectUniqueIdB)) { + PyErr_SetString(SpamError, "Error parsing object unique id"); + return NULL; } - - commandHandle = b3InitRequestContactPointInformation(sm); - b3SetContactFilterBodyA(commandHandle,objectUniqueIdA); - b3SetContactFilterBodyB(commandHandle,objectUniqueIdB); - b3SubmitClientCommand(sm, commandHandle); - int i; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED) - { - - /* - 0 int m_contactFlags; - 1 int m_bodyUniqueIdA; - 2 int m_bodyUniqueIdB; - 3 int m_linkIndexA; - 4 int m_linkIndexB; - 5-6-7 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates - 8-9-10 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates - 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A - 14 double m_contactDistance;//negative number is penetration, positive is distance. - - 15 double m_normalForce; - */ - - b3GetContactPointInformation(sm, &contactPointData); - pyResultList = PyTuple_New(contactPointData.m_numContactPoints); - for (i=0;i 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; -static PyObject* pybullet_getQuaternionFromEuler(PyObject* self, PyObject* args) -{ + double quat[4]; + + 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; + } + + { double rpy[3]; - - PyObject* eulerObj; - - if (PyArg_ParseTuple(args, "O", &eulerObj)) + 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* 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; + 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; +} + +/// Given an object id, joint positions, joint velocities and joint +/// accelerations, +/// compute the joint forces using Inverse Dynamics +static PyObject* pybullet_calculateInverseDynamics(PyObject* self, + PyObject* args) { + int size; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + size = PySequence_Size(args); + if (size == 4) { + int bodyIndex; + PyObject* objPositionsQ; + PyObject* objVelocitiesQdot; + PyObject* objAccelerations; + + if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, + &objVelocitiesQdot, &objAccelerations)) { + int szObPos = PySequence_Size(objPositionsQ); + int szObVel = PySequence_Size(objVelocitiesQdot); + int szObAcc = PySequence_Size(objAccelerations); + int numJoints = b3GetNumJoints(sm, bodyIndex); + if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && + (szObAcc == numJoints)) { + int szInBytes = sizeof(double) * numJoints; + int i; + PyObject* pylist = 0; + double* jointPositionsQ = (double*)malloc(szInBytes); + double* jointVelocitiesQdot = (double*)malloc(szInBytes); + double* jointAccelerations = (double*)malloc(szInBytes); + double* jointForcesOutput = (double*)malloc(szInBytes); + + for (i = 0; i < numJoints; i++) { + jointPositionsQ[i] = + pybullet_internalGetFloatFromSequence(objPositionsQ, i); + jointVelocitiesQdot[i] = + pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); + jointAccelerations[i] = + pybullet_internalGetFloatFromSequence(objAccelerations, i); } - Py_DECREF(seq); - } else - { - PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw]."); + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = + b3CalculateInverseDynamicsCommandInit( + sm, bodyIndex, jointPositionsQ, jointVelocitiesQdot, + jointAccelerations); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, + &dofCount, 0); + + if (dofCount) { + b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, + jointForcesOutput); + { + { + int i; + pylist = PyTuple_New(dofCount); + for (i = 0; i < dofCount; i++) + PyTuple_SetItem(pylist, i, + PyFloat_FromDouble(jointForcesOutput[i])); + } + } + } + + } else { + PyErr_SetString(SpamError, + "Internal error in calculateInverseDynamics"); + } + } + free(jointPositionsQ); + free(jointVelocitiesQdot); + free(jointAccelerations); + free(jointForcesOutput); + if (pylist) return pylist; + } else { + PyErr_SetString(SpamError, + "calculateInverseDynamics numJoints needs to be " + "positive and [joint positions], [joint velocities], " + "[joint accelerations] need to match the number of " + "joints."); return NULL; + } + + } else { + PyErr_SetString(SpamError, + "calculateInverseDynamics expects 4 arguments, body " + "index, [joint positions], [joint velocities], [joint " + "accelerations]."); + return NULL; } - - { - 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; - + } else { + PyErr_SetString(SpamError, + "calculateInverseDynamics expects 4 arguments, body index, " + "[joint positions], [joint velocities], [joint " + "accelerations]."); + return NULL; + } + Py_INCREF(Py_None); + return Py_None; } -///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; - - double quat[4]; - - 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; - } - - { - 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; -} - -///Given an object id, joint positions, joint velocities and joint accelerations, -///compute the joint forces using Inverse Dynamics -static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args) -{ - int size; - if (0 == sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - size = PySequence_Size(args); - if (size==4) - { - - int bodyIndex; - PyObject* objPositionsQ; - PyObject* objVelocitiesQdot; - PyObject* objAccelerations; - - if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, &objVelocitiesQdot, &objAccelerations)) - { - int szObPos = PySequence_Size(objPositionsQ); - int szObVel = PySequence_Size(objVelocitiesQdot); - int szObAcc = PySequence_Size(objAccelerations); - int numJoints = b3GetNumJoints(sm, bodyIndex); - if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && (szObAcc == numJoints)) - { - int szInBytes = sizeof(double)*numJoints; - int i; - PyObject* pylist = 0; - double* jointPositionsQ = (double*)malloc(szInBytes); - double* jointVelocitiesQdot = (double*)malloc(szInBytes); - double* jointAccelerations = (double*)malloc(szInBytes); - double* jointForcesOutput = (double*)malloc(szInBytes); - - for (i = 0; i < numJoints; i++) - { - jointPositionsQ[i] = pybullet_internalGetFloatFromSequence(objPositionsQ, i); - jointVelocitiesQdot[i] = pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); - jointAccelerations[i] = pybullet_internalGetFloatFromSequence(objAccelerations, i); - } - - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(sm, - bodyIndex, jointPositionsQ, jointVelocitiesQdot, jointAccelerations); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) - { - int bodyUniqueId; - int dofCount; - - b3GetStatusInverseDynamicsJointForces(statusHandle, - &bodyUniqueId, - &dofCount, - 0); - - if (dofCount) - { - b3GetStatusInverseDynamicsJointForces(statusHandle, - 0, - 0, - jointForcesOutput); - { - { - - int i; - pylist = PyTuple_New(dofCount); - for (i = 0; i= 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 */ - }; +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 */ +}; #endif PyMODINIT_FUNC @@ -1953,39 +1789,37 @@ 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, "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, "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); - - 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); #if PY_MAJOR_VERSION >= 3 - return m; + return m; #endif } - From ba2f522e05cec899d42c0493a21a7835f82f713f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 7 Sep 2016 16:02:16 -0700 Subject: [PATCH 6/6] Update BulletUrdfImporter.cpp use free (and not delete), since b3ImportMeshUtility/stbi_load uses malloc (and not new) --- examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 992840850..efbbbbb89 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1001,7 +1001,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP //delete textures for (int i=0;i