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