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

fix memory leak in PyBullet.calculateInverseKinematics when joint limits are provided

Browse Source 
Fixes Issue #2164
This commit is contained in:
Erwin Coumans
2019-03-20 15:09:48 -07:00
parent 3acac372da
commit f725d1201d
1 changed files with 174 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files

323
examples/pybullet/pybullet.c
View File

@@ -14,7 +14,6 @@
#include "../SharedMemory/physx/PhysXC_API.h"
#endif
#ifdef BT_ENABLE_MUJOCO
#include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h"
#endif
@@ -1248,7 +1247,7 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "physicsClientId", NULL};
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &physicsClientId))
{
return NULL;
@@ -1515,29 +1514,29 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep",
"numSolverIterations",
"useSplitImpulse",
"splitImpulsePenetrationThreshold",
"numSubSteps",
"collisionFilterMode",
"contactBreakingThreshold",
"maxNumCmdPer1ms",
"enableFileCaching",
"restitutionVelocityThreshold",
"erp",
"contactERP",
"frictionERP",
"enableConeFriction",
"deterministicOverlappingPairs",
"allowedCcdPenetration",
"jointFeedbackMode",
"solverResidualThreshold",
"contactSlop",
"enableSAT",
"constraintSolverType",
"globalCFM",
"minimumSolverIslandSize",
"physicsClientId", NULL};
"numSolverIterations",
"useSplitImpulse",
"splitImpulsePenetrationThreshold",
"numSubSteps",
"collisionFilterMode",
"contactBreakingThreshold",
"maxNumCmdPer1ms",
"enableFileCaching",
"restitutionVelocityThreshold",
"erp",
"contactERP",
"frictionERP",
"enableConeFriction",
"deterministicOverlappingPairs",
"allowedCcdPenetration",
"jointFeedbackMode",
"solverResidualThreshold",
"contactSlop",
"enableSAT",
"constraintSolverType",
"globalCFM",
"minimumSolverIslandSize",
"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId))
@@ -2222,7 +2221,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
if ((controlMode != CONTROL_MODE_VELOCITY) &&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)&&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD) &&
(controlMode != CONTROL_MODE_PD))
{
PyErr_SetString(SpamError, "Illegal control mode.");
@@ -2469,14 +2468,13 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
// return NULL;
}
static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject* args, PyObject* keywds)
{
int bodyUniqueId, jointIndex, controlMode;
double targetPositionArray[4] = { 0, 0, 0, 1 };
double targetVelocityArray[3] = { 0, 0, 0 };
double targetForceArray[3] = { 100000.0, 100000.0, 100000.0 };
double targetPositionArray[4] = {0, 0, 0, 1};
double targetVelocityArray[3] = {0, 0, 0};
double targetForceArray[3] = {100000.0, 100000.0, 100000.0};
int targetPositionSize = 0;
int targetVelocitySize = 0;
int targetForceSize = 0;
@@ -2490,9 +2488,9 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = { "bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL };
static char* kwlist[] = {"bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOOdddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
&targetPositionObj, &targetVelocityObj, &targetForceObj, &kp, &kd, &maxVelocity, &physicsClientId))
&targetPositionObj, &targetVelocityObj, &targetForceObj, &kp, &kd, &maxVelocity, &physicsClientId))
{
return NULL;
}
@@ -2514,7 +2512,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
{
targetPositionSize = 0;
}
if (targetPositionSize >4)
if (targetPositionSize > 4)
{
targetPositionSize = 4;
}
@@ -2539,7 +2537,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
{
targetVelocitySize = 0;
}
if (targetVelocitySize >3)
if (targetVelocitySize > 3)
{
targetVelocitySize = 3;
}
@@ -2564,7 +2562,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
{
targetForceSize = 0;
}
if (targetForceSize >3)
if (targetForceSize > 3)
{
targetForceSize = 3;
}
@@ -2578,7 +2576,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
}
}
//if (targetPositionSize == 0 && targetVelocitySize == 0)
//{
@@ -2595,11 +2592,11 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
return NULL;
}
if (//(controlMode != CONTROL_MODE_VELOCITY)&&
if ( //(controlMode != CONTROL_MODE_VELOCITY)&&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)//&&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD) //&&
//(controlMode != CONTROL_MODE_PD)
)
)
{
PyErr_SetString(SpamError, "Illegal control mode.");
return NULL;
@@ -2621,59 +2618,57 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
break;
}
#endif
case CONTROL_MODE_TORQUE:
{
if (info.m_uSize == targetForceSize)
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
targetForceArray, targetForceSize);
if (info.m_uSize == targetForceSize)
{
b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
targetForceArray, targetForceSize);
}
break;
}
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
case CONTROL_MODE_PD:
{
case CONTROL_MODE_POSITION_VELOCITY_PD:
case CONTROL_MODE_PD:
{
//make sure size == info.m_qSize
//make sure size == info.m_qSize
if (maxVelocity > 0)
{
b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity);
}
if (maxVelocity > 0)
{
b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity);
}
if (info.m_qSize == targetPositionSize)
{
b3JointControlSetDesiredPositionMultiDof(commandHandle, info.m_qIndex,
targetPositionArray, targetPositionSize);
}
else
{
//printf("Warning: targetPosition array size doesn't match joint position size (got %d, expected %d).",targetPositionSize, info.m_qSize);
}
if (info.m_qSize == targetPositionSize)
{
b3JointControlSetDesiredPositionMultiDof(commandHandle, info.m_qIndex,
targetPositionArray, targetPositionSize);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
if (info.m_uSize == targetVelocitySize)
{
b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_uIndex,
targetVelocityArray, targetVelocitySize);
}
else
{
//printf("Warning: targetVelocity array size doesn't match joint dimentions (got %d, expected %d).", targetVelocitySize, info.m_uSize);
}
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
if (info.m_uSize == targetForceSize || targetForceSize == 1)
{
b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
targetForceArray, targetForceSize);
}
break;
}
else
default:
{
//printf("Warning: targetPosition array size doesn't match joint position size (got %d, expected %d).",targetPositionSize, info.m_qSize);
}
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
if (info.m_uSize == targetVelocitySize)
{
b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_uIndex,
targetVelocityArray, targetVelocitySize);
}
else
{
//printf("Warning: targetVelocity array size doesn't match joint dimentions (got %d, expected %d).", targetVelocitySize, info.m_uSize);
}
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
if (info.m_uSize == targetForceSize || targetForceSize==1)
{
b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
targetForceArray, targetForceSize);
}
break;
}
default:
{
}
};
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
@@ -2685,7 +2680,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
// return NULL;
}
static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds)
{
int bodyUniqueId, jointIndex, controlMode;
@@ -3756,15 +3750,15 @@ static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args
b3PhysicsClientHandle sm = 0;
int bodyUniqueId;
int jointIndex;
double targetPositionArray[4] = { 0, 0, 0, 1 };
double targetVelocityArray[3] = { 0, 0, 0 };
double targetPositionArray[4] = {0, 0, 0, 1};
double targetVelocityArray[3] = {0, 0, 0};
int targetPositionSize = 0;
int targetVelocitySize = 0;
PyObject* targetPositionObj = 0;
PyObject* targetVelocityObj = 0;
int physicsClientId = 0;
static char* kwlist[] = { "bodyUniqueId", "jointIndex", "targetValue", "targetVelocity", "physicsClientId", NULL };
static char* kwlist[] = {"bodyUniqueId", "jointIndex", "targetValue", "targetVelocity", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|Oi", kwlist, &bodyUniqueId, &jointIndex, &targetPositionObj, &targetVelocityObj, &physicsClientId))
{
return NULL;
@@ -3787,7 +3781,7 @@ static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args
{
targetPositionSize = 0;
}
if (targetPositionSize >4)
if (targetPositionSize > 4)
{
targetPositionSize = 4;
}
@@ -3812,7 +3806,7 @@ static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args
{
targetVelocitySize = 0;
}
if (targetVelocitySize >3)
if (targetVelocitySize > 3)
{
targetVelocitySize = 3;
}
@@ -4250,7 +4244,6 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
PyObject* pyListVelocity;
PyObject* pyListJointMotorTorque;
struct b3JointSensorState2 sensorState;
int bodyUniqueId = -1;
@@ -4261,7 +4254,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = { "bodyUniqueId", "jointIndex", "physicsClientId", NULL };
static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &jointIndex, &physicsClientId))
{
return NULL;
@@ -4317,29 +4310,28 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
for (i = 0; i < sensorState.m_qDofSize; i++)
{
PyTuple_SetItem(pyListPosition, i,
PyFloat_FromDouble(sensorState.m_jointPosition[i]));
PyFloat_FromDouble(sensorState.m_jointPosition[i]));
}
for (i = 0; i < sensorState.m_uDofSize; i++)
{
PyTuple_SetItem(pyListVelocity, i,
PyFloat_FromDouble(sensorState.m_jointVelocity[i]));
PyFloat_FromDouble(sensorState.m_jointVelocity[i]));
PyTuple_SetItem(pyListJointMotorTorque, i,
PyFloat_FromDouble(sensorState.m_jointMotorTorqueMultiDof[i]));
PyFloat_FromDouble(sensorState.m_jointMotorTorqueMultiDof[i]));
}
for (j = 0; j < forceTorqueSize; j++)
{
PyTuple_SetItem(pyListJointForceTorque, j,
PyFloat_FromDouble(sensorState.m_jointReactionForceTorque[j]));
PyFloat_FromDouble(sensorState.m_jointReactionForceTorque[j]));
}
PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque);
return pyListJointState;
}
else
@@ -5354,9 +5346,9 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
}
}
if (parentObjectUniqueId>=0)
if (parentObjectUniqueId >= 0)
{
b3RaycastBatchSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex);
b3RaycastBatchSetParentObject(commandHandle, parentObjectUniqueId, parentLinkIndex);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
@@ -6129,7 +6121,7 @@ static PyObject* pybullet_changeVisualShape(PyObject* self, PyObject* args, PyOb
{
commandHandle = b3InitUpdateVisualShape2(sm, objectUniqueId, jointIndex, shapeIndex);
if (textureUniqueId>=-1)
if (textureUniqueId >= -1)
{
b3UpdateVisualShapeTexture(commandHandle, textureUniqueId);
}
@@ -6805,14 +6797,13 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject*
return NULL;
}
static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVertices)
{
int numVerticesOut=0;
int numVerticesOut = 0;
if (verticesObj)
{
PyObject* seqVerticesObj= PySequence_Fast(verticesObj, "expected a sequence of vertex positions");
PyObject* seqVerticesObj = PySequence_Fast(verticesObj, "expected a sequence of vertex positions");
if (seqVerticesObj)
{
int numVerticesSrc = PySequence_Size(seqVerticesObj);
@@ -6846,7 +6837,6 @@ static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVe
return numVerticesOut;
}
static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices)
{
int numUVOut = 0;
@@ -6887,11 +6877,11 @@ static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices)
}
static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices)
{
int numIndicesOut=0;
int numIndicesOut = 0;
if (indicesObj)
{
PyObject* seqIndicesObj= PySequence_Fast(indicesObj, "expected a sequence of indices");
PyObject* seqIndicesObj = PySequence_Fast(indicesObj, "expected a sequence of indices");
if (seqIndicesObj)
{
int numIndicesSrc = PySequence_Size(seqIndicesObj);
@@ -6906,7 +6896,7 @@ static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices)
}
for (i = 0; i < numIndicesSrc; i++)
{
int index = pybullet_internalGetIntFromSequence(seqIndicesObj,i);
int index = pybullet_internalGetIntFromSequence(seqIndicesObj, i);
if (indices)
{
indices[numIndicesOut] = index;
@@ -6986,8 +6976,8 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
}
if (shapeType == GEOM_MESH && verticesObj)
{
int numVertices= extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES);
int numIndices= extractIndices(indicesObj, 0, B3_MAX_NUM_INDICES);
int numVertices = extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES);
int numIndices = extractIndices(indicesObj, 0, B3_MAX_NUM_INDICES);
double* vertices = numVertices ? malloc(numVertices * 3 * sizeof(double)) : 0;
int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0;
@@ -6996,13 +6986,14 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
if (indicesObj)
{
numIndices = extractIndices(indicesObj, indices,B3_MAX_NUM_INDICES);
numIndices = extractIndices(indicesObj, indices, B3_MAX_NUM_INDICES);
}
if (numIndices)
{
shapeIndex = b3CreateCollisionShapeAddConcaveMesh(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices);
} else
}
else
{
shapeIndex = b3CreateCollisionShapeAddConvexMesh(sm, commandHandle, meshScale, vertices, numVertices);
}
@@ -7777,13 +7768,12 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
double* batchPositions = malloc(sizeof(double) * 3 * numBatchPositions);
for (i = 0; i < numBatchPositions; i++)
{
pybullet_internalGetVector3FromSequence(seqBatchPositions, i, &batchPositions[3*i]);
pybullet_internalGetVector3FromSequence(seqBatchPositions, i, &batchPositions[3 * i]);
}
b3CreateMultiBodySetBatchPositions(sm, commandHandle, batchPositions, numBatchPositions);
free(batchPositions);
}
for (i = 0; i < numLinkMasses; i++)
{
double linkMass = pybullet_internalGetFloatFromSequence(seqLinkMasses, i);
@@ -9062,7 +9052,7 @@ static PyObject* pybullet_rotateVector(PyObject* self, PyObject* args, PyObject*
int hasQuat = 0;
int hasVec = 0;
static char* kwlist[] = { "quaternion", "vector", "physicsClientId", NULL };
static char* kwlist[] = {"quaternion", "vector", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatObj, &vectorObj, &physicsClientId))
{
return NULL;
@@ -9099,7 +9089,6 @@ static PyObject* pybullet_rotateVector(PyObject* self, PyObject* args, PyObject*
return Py_None;
}
static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* quatStartObj;
@@ -9111,7 +9100,7 @@ static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject*
int hasQuatStart = 0;
int hasQuatEnd = 0;
static char* kwlist[] = { "quaternionStart", "quaternionEnd", "deltaTime", "physicsClientId", NULL };
static char* kwlist[] = {"quaternionStart", "quaternionEnd", "deltaTime", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOd|i", kwlist, &quatStartObj, &quatEndObj, &deltaTime, &physicsClientId))
{
return NULL;
@@ -9148,8 +9137,7 @@ static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject*
return Py_None;
}
static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyObject* keywds)
static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* quatStartObj;
PyObject* quatEndObj;
@@ -9160,7 +9148,7 @@ static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyO
int hasQuatStart = 0;
int hasQuatEnd = 0;
static char* kwlist[] = { "quaternionStart", "quaternionEnd", "interpolationFraction", "physicsClientId", NULL };
static char* kwlist[] = {"quaternionStart", "quaternionEnd", "interpolationFraction", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOd|i", kwlist, &quatStartObj, &quatEndObj, &interpolationFraction, &physicsClientId))
{
return NULL;
@@ -9197,7 +9185,6 @@ static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyO
return Py_None;
}
static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
@@ -9205,7 +9192,7 @@ static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* a
double quat[4];
int hasQuat = 0;
static char* kwlist[] = { "quaternion", "physicsClientId", NULL };
static char* kwlist[] = {"quaternion", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|i", kwlist, &quatObj, &physicsClientId))
{
return NULL;
@@ -9245,7 +9232,6 @@ static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* a
return Py_None;
}
static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* axisObj;
@@ -9254,8 +9240,8 @@ static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* a
int physicsClientId = 0;
int hasAxis = 0;
static char* kwlist[] = { "axis", "angle","physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "Od|i", kwlist, &axisObj, &angle,&physicsClientId))
static char* kwlist[] = {"axis", "angle", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "Od|i", kwlist, &axisObj, &angle, &physicsClientId))
{
return NULL;
}
@@ -9287,7 +9273,6 @@ static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* a
return Py_None;
}
static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* quatStartObj;
@@ -9298,7 +9283,7 @@ static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject*
int hasQuatStart = 0;
int hasQuatEnd = 0;
static char* kwlist[] = { "quaternionStart", "quaternionEnd", "physicsClientId", NULL };
static char* kwlist[] = {"quaternionStart", "quaternionEnd", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatStartObj, &quatEndObj, &physicsClientId))
{
return NULL;
@@ -9335,7 +9320,6 @@ static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject*
return Py_None;
}
static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* quatStartObj;
@@ -9346,7 +9330,7 @@ static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args
int hasQuatStart = 0;
int hasQuatEnd = 0;
static char* kwlist[] = { "quaternionStart", "quaternionEnd", "physicsClientId", NULL };
static char* kwlist[] = {"quaternionStart", "quaternionEnd", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatStartObj, &quatEndObj, &physicsClientId))
{
return NULL;
@@ -9383,8 +9367,6 @@ static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args
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,
@@ -9704,6 +9686,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
{
PyErr_SetString(SpamError,
"calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom.");
free(lowerLimits);
free(upperLimits);
free(jointRanges);
free(restPoses);
return NULL;
}
else
@@ -9795,6 +9781,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
free(currentPositions);
free(jointDamping);
free(lowerLimits);
free(upperLimits);
free(jointRanges);
free(restPoses);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
@@ -9854,12 +9845,12 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyUniqueId", "objPositions",
"objVelocities", "objAccelerations",
"flags",
"flags",
"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|ii", kwlist,
&bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations,
&flags,
&flags,
&physicsClientId))
{
static char* kwlist2[] = {"bodyIndex", "objPositions",
@@ -9885,7 +9876,6 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
int szObVel = PySequence_Size(objVelocitiesQdot);
int szObAcc = PySequence_Size(objAccelerations);
if (szObVel == szObAcc)
{
int szInBytesQ = sizeof(double) * szObPos;
@@ -9896,7 +9886,6 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot);
double* jointAccelerations = (double*)malloc(szInBytesQdot);
for (i = 0; i < szObPos; i++)
{
jointPositionsQ[i] =
@@ -9927,13 +9916,13 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
{
int bodyUniqueId;
int dofCount;
b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,&dofCount, 0);
b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0);
if (dofCount)
{
double* jointForcesOutput = (double*)malloc(sizeof(double) * dofCount);
b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,jointForcesOutput);
b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, jointForcesOutput);
{
{
int i;
@@ -10187,10 +10176,10 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
int szObPos = PySequence_Size(objPositions);
///int dofCountQ = b3GetNumJoints(sm, bodyUniqueId);
if (szObPos>=0)//(szObPos == dofCountQ))
if (szObPos >= 0) //(szObPos == dofCountQ))
{
int byteSizeJoints = sizeof(double) * szObPos;
PyObject* pyResultList=NULL;
PyObject* pyResultList = NULL;
double* jointPositions = (double*)malloc(byteSizeJoints);
double* massMatrix = NULL;
int i;
@@ -10216,7 +10205,6 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
{
int byteSizeDofCount = sizeof(double) * dofCount;
massMatrix = (double*)malloc(dofCount * byteSizeDofCount);
b3GetStatusMassMatrix(sm, statusHandle, NULL, massMatrix);
if (massMatrix)
@@ -10470,8 +10458,8 @@ static PyMethodDef SpamMethods[] = {
{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for a joint on a body."},
{ "getJointStateMultiDof", (PyCFunction)pybullet_getJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for a joint on a body. (supports planar and spherical joints)" },
{"getJointStateMultiDof", (PyCFunction)pybullet_getJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for a joint on a body. (supports planar and spherical joints)"},
{"getJointStates", (PyCFunction)pybullet_getJointStates, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for multiple joints on a body."},
@@ -10492,10 +10480,10 @@ static PyMethodDef SpamMethods[] = {
"Reset the state (position, velocity etc) for a joint on a body "
"instantaneously, not through physics simulation."},
{ "resetJointStateMultiDof", (PyCFunction)pybullet_resetJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
"resetJointStateMultiDof(objectUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0)\n"
"Reset the state (position, velocity etc) for a joint on a body "
"instantaneously, not through physics simulation." },
{"resetJointStateMultiDof", (PyCFunction)pybullet_resetJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
"resetJointStateMultiDof(objectUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0)\n"
"Reset the state (position, velocity etc) for a joint on a body "
"instantaneously, not through physics simulation."},
{"changeDynamics", (PyCFunction)pybullet_changeDynamicsInfo, METH_VARARGS | METH_KEYWORDS,
"change dynamics information such as mass, lateral friction coefficient."},
@@ -10512,10 +10500,10 @@ 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."},
{ "setJointMotorControlMultiDof", (PyCFunction)pybullet_setJointMotorControlMultiDof, METH_VARARGS | METH_KEYWORDS,
"Set a single joint motor control mode and desired target value. There is "
"no immediate state change, stepSimulation will process the motors."
"This method sets multi-degree-of-freedom motor such as the spherical joint motor." },
{"setJointMotorControlMultiDof", (PyCFunction)pybullet_setJointMotorControlMultiDof, METH_VARARGS | METH_KEYWORDS,
"Set a single joint motor control mode and desired target value. There is "
"no immediate state change, stepSimulation will process the motors."
"This method sets multi-degree-of-freedom motor such as the spherical joint motor."},
{"setJointMotorControlArray", (PyCFunction)pybullet_setJointMotorControlArray, METH_VARARGS | METH_KEYWORDS,
"Set an array of motors control mode and desired target value. There is "
@@ -10650,30 +10638,26 @@ static PyMethodDef SpamMethods[] = {
{"getMatrixFromQuaternion", (PyCFunction)pybullet_getMatrixFromQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"},
{ "getQuaternionSlerp", (PyCFunction)pybullet_getQuaternionSlerp, METH_VARARGS | METH_KEYWORDS,
"Compute the spherical interpolation given a start and end quaternion and an interpolation value in range [0..1]" },
{"getQuaternionSlerp", (PyCFunction)pybullet_getQuaternionSlerp, METH_VARARGS | METH_KEYWORDS,
"Compute the spherical interpolation given a start and end quaternion and an interpolation value in range [0..1]"},
{ "getQuaternionFromAxisAngle", (PyCFunction)pybullet_getQuaternionFromAxisAngle, METH_VARARGS | METH_KEYWORDS,
"Compute the quaternion from axis and angle representation." },
{"getQuaternionFromAxisAngle", (PyCFunction)pybullet_getQuaternionFromAxisAngle, METH_VARARGS | METH_KEYWORDS,
"Compute the quaternion from axis and angle representation."},
{ "getAxisAngleFromQuaternion", (PyCFunction)pybullet_getAxisAngleFromQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the quaternion from axis and angle representation." },
{"getAxisAngleFromQuaternion", (PyCFunction)pybullet_getAxisAngleFromQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the quaternion from axis and angle representation."},
{ "getDifferenceQuaternion", (PyCFunction)pybullet_getDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the quaternion difference from two quaternions." },
{"getDifferenceQuaternion", (PyCFunction)pybullet_getDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the quaternion difference from two quaternions."},
{ "getAxisDifferenceQuaternion", (PyCFunction)pybullet_getAxisDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the velocity axis difference from two quaternions." },
{ "calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the angular velocity given start and end quaternion and delta time." },
{ "rotateVector", (PyCFunction)pybullet_rotateVector, METH_VARARGS | METH_KEYWORDS,
"Rotate a vector using a quaternion." },
{"getAxisDifferenceQuaternion", (PyCFunction)pybullet_getAxisDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the velocity axis difference from two quaternions."},
{"calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the angular velocity given start and end quaternion and delta time."},
{"rotateVector", (PyCFunction)pybullet_rotateVector, METH_VARARGS | METH_KEYWORDS,
"Rotate a vector using a quaternion."},
{"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS,
"Given an object id, joint positions, joint velocities and joint "
@@ -10972,7 +10956,6 @@ initpybullet(void)
PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL", URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL);
PyModule_AddIntConstant(m, "URDF_MAINTAIN_LINK_ORDER", URDF_MAINTAIN_LINK_ORDER);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp);
@@ -11039,9 +11022,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "AddFileIOAction", eAddFileIOAction);
PyModule_AddIntConstant(m, "RemoveFileIOAction", eRemoveFileIOAction);
PyModule_AddIntConstant(m, "PosixFileIO", ePosixFileIO );
PyModule_AddIntConstant(m, "ZipFileIO", eZipFileIO );
PyModule_AddIntConstant(m, "CNSFileIO", eCNSFileIO );
PyModule_AddIntConstant(m, "PosixFileIO", ePosixFileIO);
PyModule_AddIntConstant(m, "ZipFileIO", eZipFileIO);
PyModule_AddIntConstant(m, "CNSFileIO", eCNSFileIO);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);