Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1111 from erwincoumans/master

Browse Source 
fix hash functions,  pybullet: implement setJointMotorControlArray
This commit is contained in:
erwincoumans
2017-05-09 23:56:43 +00:00
committed by GitHub
parent beec3f1025 0d3940b8bd
commit 7da4a362e1
4 changed files with 288 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

276
examples/pybullet/pybullet.c
View File

@@ -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<numControlledDofs;i++)
{
double targetVelocity = 0.0;
if (targetVelocitiesSeq)
{
targetVelocity = pybullet_internalGetFloatFromSequence(targetVelocitiesSeq, i);
}
double targetPosition = 0.0;
if (targetPositionsSeq)
{
targetPosition = pybullet_internalGetFloatFromSequence(targetPositionsSeq, i);
}
double force = 100000.0;
if (forcesSeq)
{
force = pybullet_internalGetFloatFromSequence(forcesSeq, i);
}
double kp = 0.1;
if (kpsSeq)
{
kp = pybullet_internalGetFloatFromSequence(kpsSeq, i);
}
double kd = 1.0;
if (kdsSeq)
{
kd = pybullet_internalGetFloatFromSequence(kdsSeq, i);
}
int jointIndex = pybullet_internalGetFloatFromSequence(jointIndicesSeq, i);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,
targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
break;
}
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex,
force);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex,
targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,
targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
break;
}
default:
{
}
};
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_DECREF(jointIndicesSeq);
Py_INCREF(Py_None);
return Py_None;
}
// PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
// return NULL;
}
static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds)
{
int bodyIndex, jointIndex, controlMode;
@@ -5654,6 +5922,14 @@ static PyMethodDef SpamMethods[] = {
"Set a single joint motor control mode and desired target value. There is "
"no immediate state change, stepSimulation will process the motors."},
{"setJointMotorControlArray", (PyCFunction)pybullet_setJointMotorControlArray, METH_VARARGS | METH_KEYWORDS,
"Set an array of motors control mode and desired target value. There is "
"no immediate state change, stepSimulation will process the motors."
"This is similar to setJointMotorControl2, with jointIndices as a list, and optional targetPositions, "
"targetVelocities, forces, kds and kps as lists"
"Using setJointMotorControlArray has the benefit of lower calling overhead."
},
{"applyExternalForce", (PyCFunction)pybullet_applyExternalForce, METH_VARARGS | METH_KEYWORDS,
"for objectUniqueId, linkIndex (-1 for base/root link), apply a force "
"[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or "

9
src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
View File

@@ -215,10 +215,9 @@ private:
*/
SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
{
int key = static_cast<int>(((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<unsigned int>(key);
return key;
}
SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash)
{
int proxyId1 = proxy0->getUid();

6
src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
View File

@@ -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<int>(((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<unsigned int>(key);
return key;
}

12
src/LinearMath/btHashMap.h
View File

@@ -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;