diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 037f5b181..d6ae00166 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1163,6 +1163,274 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) return NULL; } +static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyIndex, controlMode; + PyObject* jointIndicesObj = 0; + PyObject* targetPositionsObj = 0; + PyObject* targetVelocitiesObj = 0; + PyObject* forcesObj = 0; + PyObject* kpsObj = 0; + PyObject* kdsObj = 0; + + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = {"bodyIndex", "jointIndices", "controlMode", "targetPositions", "targetVelocities", "forces", "positionGains", "velocityGains", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOi|OOOOOi", kwlist, &bodyIndex, &jointIndicesObj, &controlMode, + &targetPositionsObj, &targetVelocitiesObj, &forcesObj, &kpsObj, &kdsObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + + int numJoints; + int i; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; + int numControlledDofs = 0; + PyObject* jointIndicesSeq = 0; + PyObject* targetVelocitiesSeq = 0; + PyObject* targetPositionsSeq = 0; + PyObject* forcesSeq = 0; + PyObject* kpsSeq = 0; + PyObject* kdsSeq = 0; + + numJoints = b3GetNumJoints(sm, bodyIndex); + + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; + } + + jointIndicesSeq = PySequence_Fast(jointIndicesObj, "expected a sequence of joint indices"); + + if (jointIndicesSeq==0) + { + PyErr_SetString(SpamError, "expected a sequence of joint indices"); + return NULL; + } + + numControlledDofs = PySequence_Size(jointIndicesObj); + if (numControlledDofs==0) + { + Py_DECREF(jointIndicesSeq); + Py_INCREF(Py_None); + return Py_None; + } + + { + int i; + for (i = 0; i < numControlledDofs; i++) + { + int jointIndex = pybullet_internalGetFloatFromSequence(jointIndicesSeq, i); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + Py_DECREF(jointIndicesSeq); + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } + } + } + + if (targetVelocitiesObj) + { + int num = PySequence_Size(targetVelocitiesObj); + if (num != numControlledDofs) + { + Py_DECREF(jointIndicesSeq); + PyErr_SetString(SpamError, "number of target velocies should match the number of joint indices"); + return NULL; + } + targetVelocitiesSeq = PySequence_Fast(targetVelocitiesObj, "expected a sequence of target velocities"); + } + + if (targetPositionsObj) + { + int num = PySequence_Size(targetPositionsObj); + if (num != numControlledDofs) + { + Py_DECREF(jointIndicesSeq); + if (targetVelocitiesSeq) + { + Py_DECREF(targetVelocitiesSeq); + } + PyErr_SetString(SpamError, "number of target positions should match the number of joint indices"); + return NULL; + } + + targetPositionsSeq = PySequence_Fast(targetPositionsObj, "expected a sequence of target positions"); + } + + if (forcesObj) + { + int num = PySequence_Size(forcesObj); + if (num != numControlledDofs) + { + Py_DECREF(jointIndicesSeq); + if (targetVelocitiesSeq) + { + Py_DECREF(targetVelocitiesSeq); + } + if (targetPositionsSeq) + { + Py_DECREF(targetPositionsSeq); + } + + PyErr_SetString(SpamError, "number of forces should match the joint indices"); + return NULL; + } + + forcesSeq = PySequence_Fast(forcesObj, "expected a sequence of forces"); + } + + + if (kpsObj) + { + int num = PySequence_Size(kpsObj); + if (num != numControlledDofs) + { + Py_DECREF(jointIndicesSeq); + if (targetVelocitiesSeq) + { + Py_DECREF(targetVelocitiesSeq); + } + if (targetPositionsSeq) + { + Py_DECREF(targetPositionsSeq); + } + if (forcesSeq) + { + Py_DECREF(forcesSeq); + } + + PyErr_SetString(SpamError, "number of kps should match the joint indices"); + return NULL; + } + + kpsSeq = PySequence_Fast(kpsObj, "expected a sequence of kps"); + } + + + if (kdsObj) + { + int num = PySequence_Size(kdsObj); + if (num != numControlledDofs) + { + Py_DECREF(jointIndicesSeq); + if (targetVelocitiesSeq) + { + Py_DECREF(targetVelocitiesSeq); + } + if (targetPositionsSeq) + { + Py_DECREF(targetPositionsSeq); + } + if (forcesSeq) + { + Py_DECREF(forcesSeq); + } + if (kpsSeq) + { + Py_DECREF(kpsSeq); + } + PyErr_SetString(SpamError, "number of kds should match the number of joint indices"); + return NULL; + } + + kdsSeq = PySequence_Fast(kdsObj, "expected a sequence of kds"); + } + + commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); + + for (i=0;i