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

Fixes Issue #2164

examples/pybullet/pybullet.c

@@ -14,7 +14,6 @@
 #include "../SharedMemory/physx/PhysXC_API.h"
 #endif
 
-
 #ifdef BT_ENABLE_MUJOCO
 #include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h"
 #endif
@@ -2469,7 +2468,6 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
 	//  return NULL;
 }
 
-
 static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int bodyUniqueId, jointIndex, controlMode;
@@ -2578,7 +2576,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 		}
 	}
 
-
 	//if (targetPositionSize == 0 && targetVelocitySize == 0)
 	//{
 
@@ -2621,7 +2618,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 			break;
 		}
 
-		
 #endif
 			case CONTROL_MODE_TORQUE:
 			{
@@ -2635,7 +2631,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 			case CONTROL_MODE_POSITION_VELOCITY_PD:
 			case CONTROL_MODE_PD:
 			{
-
 				//make sure size == info.m_qSize
 
 				if (maxVelocity > 0)
@@ -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;
@@ -4250,7 +4244,6 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
 	PyObject* pyListVelocity;
 	PyObject* pyListJointMotorTorque;
 
-
 	struct b3JointSensorState2 sensorState;
 
 	int bodyUniqueId = -1;
@@ -4339,7 +4332,6 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
 
 				PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque);
 
-
 				return pyListJointState;
 			}
 			else
@@ -6805,7 +6797,6 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject*
 	return NULL;
 }
 
-
 static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVertices)
 {
 	int numVerticesOut = 0;
@@ -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;
@@ -7002,7 +6992,8 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 			if (numIndices)
 			{
 				shapeIndex = b3CreateCollisionShapeAddConcaveMesh(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices);
-			} else
+			}
+			else
 			{
 				shapeIndex = b3CreateCollisionShapeAddConvexMesh(sm, commandHandle, meshScale, vertices, numVertices);
 			}
@@ -7783,7 +7774,6 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
 				free(batchPositions);
 			}
 
-
 			for (i = 0; i < numLinkMasses; i++)
 			{
 				double linkMass = pybullet_internalGetFloatFromSequence(seqLinkMasses, i);
@@ -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;
@@ -9148,7 +9137,6 @@ static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject*
 	return Py_None;
 }
 
-
 static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	PyObject* quatStartObj;
@@ -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;
@@ -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;
@@ -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;
@@ -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;
@@ -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,
@@ -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] =
@@ -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)
@@ -10665,16 +10653,12 @@ static PyMethodDef SpamMethods[] = {
 	{"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 "
 	 "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_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);