diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 1b3cac5c4..726abebac 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1220,6 +1220,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(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); + unsigned int key = proxyId1 | (proxyId2 << 16); // Thomas Wang's hash key += ~(key << 15); @@ -227,13 +226,11 @@ private: key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); - return static_cast(key); + return key; } - - SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash) { int proxyId1 = proxy0->getUid(); diff --git a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index 186964d72..2aaf6201f 100644 --- a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -123,9 +123,9 @@ private: - SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) + SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) { - int key = static_cast(((unsigned int)indexA) | (((unsigned int)indexB) <<16)); + unsigned int key = indexA | (indexB << 16); // Thomas Wang's hash key += ~(key << 15); @@ -134,7 +134,7 @@ private: key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); - return static_cast(key); + return key; } diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index 8f723a646..3e8b75b7d 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -105,9 +105,10 @@ public: //to our success SIMD_FORCE_INLINE unsigned int getHash()const { - int key = m_uid; + unsigned int key = m_uid; // Thomas Wang's hash key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); + return key; } }; @@ -120,7 +121,7 @@ class btHashPtr union { const void* m_pointer; - int m_hashValues[2]; + unsigned int m_hashValues[2]; }; public: @@ -145,8 +146,7 @@ public: { const bool VOID_IS_8 = ((sizeof(void*)==8)); - int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0]; - + unsigned int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0]; // Thomas Wang's hash key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); return key; @@ -179,7 +179,7 @@ public: //to our success SIMD_FORCE_INLINE unsigned int getHash()const { - int key = m_uid; + unsigned int key = m_uid; // Thomas Wang's hash key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); return key; @@ -211,7 +211,7 @@ public: //to our success SIMD_FORCE_INLINE unsigned int getHash()const { - int key = m_uid; + unsigned int key = m_uid; // Thomas Wang's hash key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); return key;