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

39
examples/pybullet/pybullet.c
View File

@@ -14,7 +14,6 @@
#include "../SharedMemory/physx/PhysXC_API.h" #include "../SharedMemory/physx/PhysXC_API.h"
#endif #endif
#ifdef BT_ENABLE_MUJOCO #ifdef BT_ENABLE_MUJOCO
#include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h" #include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h"
#endif #endif
@@ -2469,7 +2468,6 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
// return NULL; // return NULL;
} }
static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject* args, PyObject* keywds)
{ {
int bodyUniqueId, jointIndex, controlMode; int bodyUniqueId, jointIndex, controlMode;
@@ -2578,7 +2576,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
} }
} }
//if (targetPositionSize == 0 && targetVelocitySize == 0) //if (targetPositionSize == 0 && targetVelocitySize == 0)
//{ //{
@@ -2621,7 +2618,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
break; break;
} }
#endif #endif
case CONTROL_MODE_TORQUE: case CONTROL_MODE_TORQUE:
{ {
@@ -2635,7 +2631,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
case CONTROL_MODE_POSITION_VELOCITY_PD: case CONTROL_MODE_POSITION_VELOCITY_PD:
case CONTROL_MODE_PD: case CONTROL_MODE_PD:
{ {
//make sure size == info.m_qSize //make sure size == info.m_qSize
if (maxVelocity > 0) if (maxVelocity > 0)
@@ -2685,7 +2680,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
// return NULL; // return NULL;
} }
static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds)
{ {
int bodyUniqueId, jointIndex, controlMode; int bodyUniqueId, jointIndex, controlMode;
@@ -4250,7 +4244,6 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
PyObject* pyListVelocity; PyObject* pyListVelocity;
PyObject* pyListJointMotorTorque; PyObject* pyListJointMotorTorque;
struct b3JointSensorState2 sensorState; struct b3JointSensorState2 sensorState;
int bodyUniqueId = -1; int bodyUniqueId = -1;
@@ -4339,7 +4332,6 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque); PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque);
return pyListJointState; return pyListJointState;
} }
else else
@@ -6805,7 +6797,6 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject*
return NULL; return NULL;
} }
static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVertices) static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVertices)
{ {
int numVerticesOut = 0; int numVerticesOut = 0;
@@ -6846,7 +6837,6 @@ static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVe
return numVerticesOut; return numVerticesOut;
} }
static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices) static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices)
{ {
int numUVOut = 0; int numUVOut = 0;
@@ -7002,7 +6992,8 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
if (numIndices) if (numIndices)
{ {
shapeIndex = b3CreateCollisionShapeAddConcaveMesh(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices); shapeIndex = b3CreateCollisionShapeAddConcaveMesh(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices);
} else }
else
{ {
shapeIndex = b3CreateCollisionShapeAddConvexMesh(sm, commandHandle, meshScale, vertices, numVertices); shapeIndex = b3CreateCollisionShapeAddConvexMesh(sm, commandHandle, meshScale, vertices, numVertices);
} }
@@ -7783,7 +7774,6 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
free(batchPositions); free(batchPositions);
} }
for (i = 0; i < numLinkMasses; i++) for (i = 0; i < numLinkMasses; i++)
{ {
double linkMass = pybullet_internalGetFloatFromSequence(seqLinkMasses, i); double linkMass = pybullet_internalGetFloatFromSequence(seqLinkMasses, i);
@@ -9099,7 +9089,6 @@ static PyObject* pybullet_rotateVector(PyObject* self, PyObject* args, PyObject*
return Py_None; return Py_None;
} }
static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{ {
PyObject* quatStartObj; PyObject* quatStartObj;
@@ -9148,7 +9137,6 @@ static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject*
return Py_None; 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* quatStartObj;
@@ -9197,7 +9185,6 @@ static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyO
return Py_None; return Py_None;
} }
static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{ {
int physicsClientId = 0; int physicsClientId = 0;
@@ -9245,7 +9232,6 @@ static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* a
return Py_None; return Py_None;
} }
static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* args, PyObject* keywds)
{ {
PyObject* axisObj; PyObject* axisObj;
@@ -9287,7 +9273,6 @@ static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* a
return Py_None; return Py_None;
} }
static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{ {
PyObject* quatStartObj; PyObject* quatStartObj;
@@ -9335,7 +9320,6 @@ static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject*
return Py_None; return Py_None;
} }
static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{ {
PyObject* quatStartObj; PyObject* quatStartObj;
@@ -9383,8 +9367,6 @@ static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args
return Py_None; return Py_None;
} }
/// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo /// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc /// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
@@ -9704,6 +9686,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
{ {
PyErr_SetString(SpamError, PyErr_SetString(SpamError,
"calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom."); "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; return NULL;
} }
else else
@@ -9795,6 +9781,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
free(currentPositions); free(currentPositions);
free(jointDamping); free(jointDamping);
free(lowerLimits);
free(upperLimits);
free(jointRanges);
free(restPoses);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle, result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
@@ -9885,7 +9876,6 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
int szObVel = PySequence_Size(objVelocitiesQdot); int szObVel = PySequence_Size(objVelocitiesQdot);
int szObAcc = PySequence_Size(objAccelerations); int szObAcc = PySequence_Size(objAccelerations);
if (szObVel == szObAcc) if (szObVel == szObAcc)
{ {
int szInBytesQ = sizeof(double) * szObPos; int szInBytesQ = sizeof(double) * szObPos;
@@ -9896,7 +9886,6 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot); double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot);
double* jointAccelerations = (double*)malloc(szInBytesQdot); double* jointAccelerations = (double*)malloc(szInBytesQdot);
for (i = 0; i < szObPos; i++) for (i = 0; i < szObPos; i++)
{ {
jointPositionsQ[i] = jointPositionsQ[i] =
@@ -10216,7 +10205,6 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
{ {
int byteSizeDofCount = sizeof(double) * dofCount; int byteSizeDofCount = sizeof(double) * dofCount;
massMatrix = (double*)malloc(dofCount * byteSizeDofCount); massMatrix = (double*)malloc(dofCount * byteSizeDofCount);
b3GetStatusMassMatrix(sm, statusHandle, NULL, massMatrix); b3GetStatusMassMatrix(sm, statusHandle, NULL, massMatrix);
if (massMatrix) if (massMatrix)
@@ -10665,16 +10653,12 @@ static PyMethodDef SpamMethods[] = {
{"getAxisDifferenceQuaternion", (PyCFunction)pybullet_getAxisDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS, {"getAxisDifferenceQuaternion", (PyCFunction)pybullet_getAxisDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the velocity axis difference from two quaternions."}, "Compute the velocity axis difference from two quaternions."},
{"calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS, {"calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the angular velocity given start and end quaternion and delta time."}, "Compute the angular velocity given start and end quaternion and delta time."},
{"rotateVector", (PyCFunction)pybullet_rotateVector, METH_VARARGS | METH_KEYWORDS, {"rotateVector", (PyCFunction)pybullet_rotateVector, METH_VARARGS | METH_KEYWORDS,
"Rotate a vector using a quaternion."}, "Rotate a vector using a quaternion."},
{"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS, {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS,
"Given an object id, joint positions, joint velocities and joint " "Given an object id, joint positions, joint velocities and joint "
"accelerations, compute the joint forces using Inverse Dynamics"}, "accelerations, compute the joint forces using Inverse Dynamics"},
@@ -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_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, "URDF_MAINTAIN_LINK_ORDER", URDF_MAINTAIN_LINK_ORDER);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping); PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping); PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp); PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp);