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

Fixes Issue #2164

1 files changed, 174 insertions, 191 deletions

diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index dfd95e524..26f73083e 100644
--- a/examples/pybullet/pybullet.c
+++ b/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
@@ -1233,7 +1232,7 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 	double restitution = -1;
 	double linearDamping = -1;
 	double angularDamping = -1;
-	
+
 	double contactStiffness = -1;
 	double contactDamping = -1;
 	double ccdSweptSphereRadius = -1;
@@ -1244,11 +1243,11 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 	PyObject* localInertiaDiagonalObj = 0;
 	PyObject* anisotropicFrictionObj = 0;
 	double maxJointVelocity = -1;
-	
+
 	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;
@@ -1341,7 +1340,7 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 		{
 			b3ChangeDynamicsInfoSetMaxJointVelocity(command, bodyUniqueId, maxJointVelocity);
 		}
-				
+
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	}
 
@@ -1514,30 +1513,30 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 	int minimumSolverIslandSize = -1;
 
 	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};
+	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};
 
 	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,30 +2468,29 @@ 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;
 	PyObject* targetPositionObj = 0;
 	PyObject* targetVelocityObj = 0;
 	PyObject* targetForceObj = 0;
-		
+
 	double kp = 0.1;
 	double kd = 1.0;
 	double maxVelocity = -1;
 	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;
@@ -2608,7 +2605,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 		commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, controlMode);
 
 		b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);
-				
+
 		switch (controlMode)
 		{
 #if 0
@@ -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:
-		{
-
-			//make sure size == info.m_qSize
-			
-			if (maxVelocity > 0)
+			case CONTROL_MODE_POSITION_VELOCITY_PD:
+			case CONTROL_MODE_PD:
 			{
-				b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity);
-			}
+				//make sure size == info.m_qSize
 
-			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);
-			}
-			
-			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);
+				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);
+				}
+
+				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;
 			}
-			b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
-			if (info.m_uSize == targetForceSize || targetForceSize==1)
+			default:
 			{
-				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;
 			}
@@ -3800,7 +3794,7 @@ static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args
 				Py_DECREF(targetPositionSeq);
 			}
 		}
-		
+
 		if (targetVelocityObj)
 		{
 			int i = 0;
@@ -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,28 +4310,27 @@ 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]));
+					PyTuple_SetItem(pyListJointMotorTorque, 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;
 			}
@@ -5265,7 +5257,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
 	int sizeTo = 0;
 	int parentObjectUniqueId = -1;
 	int parentLinkIndex = -1;
-	
+
 	static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL};
 	int physicsClientId = 0;
 
@@ -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);
 		}
@@ -6438,7 +6430,7 @@ static PyObject* pybullet_setCollisionFilterGroupMask(PyObject* self, PyObject*
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	
+
 	commandHandle = b3CollisionFilterCommandInit(sm);
 	b3SetCollisionFilterGroupMask(commandHandle, bodyUniqueIdA, linkIndexA, collisionFilterGroup, collisionFilterMask);
 
@@ -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,23 +6976,24 @@ 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;
 
 			numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES);
 			pybullet_internalSetVectord(meshScaleObj, meshScale);
-						
+
 			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,12 +7768,11 @@ 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++)
 			{
@@ -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;
@@ -9232,7 +9219,7 @@ static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* a
 				PyTuple_SetItem(pylist2, 0, axislist);
 			}
 			PyTuple_SetItem(pylist2, 1, PyFloat_FromDouble(angle));
-			
+
 			return pylist2;
 		}
 	}
@@ -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;
@@ -9345,8 +9329,8 @@ static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args
 	int physicsClientId = 0;
 	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;
@@ -9895,7 +9885,6 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 				double* jointPositionsQ = (double*)malloc(szInBytesQ);
 				double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot);
 				double* jointAccelerations = (double*)malloc(szInBytesQdot);
-				
 
 				for (i = 0; i < szObPos; 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;
@@ -9955,7 +9944,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 				free(jointPositionsQ);
 				free(jointVelocitiesQdot);
 				free(jointAccelerations);
-				
+
 				if (pylist) return pylist;
 			}
 			else
@@ -10186,11 +10175,11 @@ 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;
@@ -10215,7 +10204,6 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
 						if (dofCount)
 						{
 							int byteSizeDofCount = sizeof(double) * dofCount;
-							
 
 							massMatrix = (double*)malloc(dofCount * byteSizeDofCount);
 							b3GetStatusMassMatrix(sm, statusHandle, NULL, 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." },
+	{"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."},
 
-	 { "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." },
-
-		
+	{"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 "
@@ -10971,8 +10955,7 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_COLORS_FROM_MTL", URDF_USE_MATERIAL_COLORS_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, "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);