Merge pull request #2359 from erwincoumans/master

premake add enable_stable_pd option, so Bullet can be compiled withou…

build3/premake4.lua

@@ -54,6 +54,13 @@
 		description = "Try to link and use system X11 headers instead of dynamically loading X11 (dlopen is default)"
 	}
 
+	newoption
+	{
+		trigger = "enable_stable_pd",
+		description = "Enable Stable PD control in PyBullet"
+	}
+
+
 	newoption
 	{
 		trigger = "enable_static_vr_plugin",

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -660,6 +660,27 @@ btTransform ConvertURDF2BulletInternal(
 
 				if (mbLinkIndex >= 0)  //???? double-check +/- 1
 				{
+					//if the base is static and all joints in the chain between this link and the base are fixed, 
+					//then this link is static too (doesn't merge islands)
+					if (cache.m_bulletMultiBody->getBaseMass() == 0)
+					{
+						bool allJointsFixed = true;
+						int testLinkIndex = mbLinkIndex;
+						do
+						{
+							if (cache.m_bulletMultiBody->getLink(testLinkIndex).m_jointType != btMultibodyLink::eFixed)
+							{
+								allJointsFixed = false;
+								break;
+							}
+							testLinkIndex = cache.m_bulletMultiBody->getLink(testLinkIndex).m_parent;
+						} while (testLinkIndex> 0);
+						if (allJointsFixed)
+						{
+							col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
+						}
+
+					}
 					cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider = col;
 					if (flags & CUF_USE_SELF_COLLISION_INCLUDE_PARENT)
 					{

examples/pybullet/premake4.lua

@@ -17,7 +17,8 @@ project ("pybullet")
 		
 		includedirs {"../../src", "../../examples",
 		"../../examples/ThirdPartyLibs"}
-		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER", "STATIC_LINK_SPD_PLUGIN"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+		
 		
 		
 	hasCL = findOpenCL("clew")
@@ -181,6 +182,12 @@ if not _OPTIONS["no-enet"] then
 			"../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
+			
+			
+
+	if _OPTIONS["enable_stable_pd"] then
+		defines {"STATIC_LINK_SPD_PLUGIN"}
+		files {
 			"../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp",
 			"../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h",
 			"../../examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp",
@@ -196,7 +203,9 @@ if not _OPTIONS["no-enet"] then
 			"../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp",
 			"../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h",
 			}
-
+		end
+		
+		
 	if _OPTIONS["enable_physx"] then
   	defines {"BT_ENABLE_PHYSX","PX_PHYSX_STATIC_LIB", "PX_FOUNDATION_DLL=0"}
 		

examples/pybullet/pybullet.c

@@ -4319,6 +4319,12 @@ static PyObject* pybullet_resetJointStatesMultiDof(PyObject* self, PyObject* arg
 				targetVelocitiesSeq = PySequence_Fast(targetVelocitiesObj, "expected a sequence of target positions");
 			for (i = 0; i < numIndices; i++)
 			{
+				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 jointIndex = pybullet_internalGetIntFromSequence(jointIndicesSeq, i);
 				if ((jointIndex >= numJoints) || (jointIndex < 0))
@@ -4333,12 +4339,7 @@ static PyObject* pybullet_resetJointStatesMultiDof(PyObject* self, PyObject* arg
 				}
 
 
-				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;
 				
 
 				if (numTargetPositionObjs > 0)
@@ -6069,6 +6070,7 @@ static PyObject* pybullet_submitProfileTiming(PyObject* self, PyObject* args, Py
 	b3PhysicsClientHandle sm = 0;
 	static char* kwlist[] = {"eventName ", "physicsClientId", NULL};
 	int physicsClientId = 0;
+	b3SharedMemoryCommandHandle commandHandle;
 
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|si", kwlist,
 									 &eventName, &physicsClientId))
@@ -6080,7 +6082,7 @@ static PyObject* pybullet_submitProfileTiming(PyObject* self, PyObject* args, Py
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	b3SharedMemoryCommandHandle commandHandle;
+	
 	commandHandle = b3ProfileTimingCommandInit(sm, eventName);
 
 	if (eventName)
@@ -10979,10 +10981,12 @@ static PyObject* pybullet_calculateInverseKinematics2(PyObject* self,
 	}
 	{
 		int numEndEffectorPositions = extractVertices(targetPosObj, 0, B3_MAX_NUM_END_EFFECTORS);
+		    
 		int numIndices = extractIndices(endEffectorLinkIndicesObj, 0, B3_MAX_NUM_END_EFFECTORS);
 		double* positions = numEndEffectorPositions ? malloc(numEndEffectorPositions * 3 * sizeof(double)) : 0;
 		int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0;
 
+
 		numEndEffectorPositions = extractVertices(targetPosObj, positions, B3_MAX_NUM_VERTICES);
 		
 		if (endEffectorLinkIndicesObj)
@@ -10990,197 +10994,199 @@ static PyObject* pybullet_calculateInverseKinematics2(PyObject* self,
 			numIndices = extractIndices(endEffectorLinkIndicesObj, indices, B3_MAX_NUM_INDICES);
 		}
 		
-		double pos[3] = { 0, 0, 0 };
-		double ori[4] = { 0, 0, 0, 1 };
-		int hasPos = numEndEffectorPositions > 0;
-		int hasOrn = 0;// pybullet_internalSetVector4d(targetOrnObj, ori);
-
-		int szLowerLimits = lowerLimitsObj ? PySequence_Size(lowerLimitsObj) : 0;
-		int szUpperLimits = upperLimitsObj ? PySequence_Size(upperLimitsObj) : 0;
-		int szJointRanges = jointRangesObj ? PySequence_Size(jointRangesObj) : 0;
-		int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0;
-		int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0;
-
-		int szCurrentPositions = currentPositionsObj ? PySequence_Size(currentPositionsObj) : 0;
-
-		int numJoints = b3GetNumJoints(sm, bodyUniqueId);
-		int dofCount = b3ComputeDofCount(sm, bodyUniqueId);
-
-		int hasNullSpace = 0;
-		int hasJointDamping = 0;
-		int hasCurrentPositions = 0;
-		double* lowerLimits = 0;
-		double* upperLimits = 0;
-		double* jointRanges = 0;
-		double* restPoses = 0;
-		double* jointDamping = 0;
-		double* currentPositions = 0;
-
-		if (dofCount && (szLowerLimits == dofCount) && (szUpperLimits == dofCount) &&
-			(szJointRanges == dofCount) && (szRestPoses == dofCount))
 		{
-			int szInBytes = sizeof(double) * dofCount;
+			double pos[3] = { 0, 0, 0 };
-			int i;
+			double ori[4] = { 0, 0, 0, 1 };
-			lowerLimits = (double*)malloc(szInBytes);
+			int hasPos = numEndEffectorPositions > 0;
-			upperLimits = (double*)malloc(szInBytes);
+			int hasOrn = 0;// pybullet_internalSetVector4d(targetOrnObj, ori);
-			jointRanges = (double*)malloc(szInBytes);
-			restPoses = (double*)malloc(szInBytes);
 
-			for (i = 0; i < dofCount; i++)
+			int szLowerLimits = lowerLimitsObj ? PySequence_Size(lowerLimitsObj) : 0;
+			int szUpperLimits = upperLimitsObj ? PySequence_Size(upperLimitsObj) : 0;
+			int szJointRanges = jointRangesObj ? PySequence_Size(jointRangesObj) : 0;
+			int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0;
+			int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0;
+
+			int szCurrentPositions = currentPositionsObj ? PySequence_Size(currentPositionsObj) : 0;
+
+			int numJoints = b3GetNumJoints(sm, bodyUniqueId);
+			int dofCount = b3ComputeDofCount(sm, bodyUniqueId);
+
+			int hasNullSpace = 0;
+			int hasJointDamping = 0;
+			int hasCurrentPositions = 0;
+			double* lowerLimits = 0;
+			double* upperLimits = 0;
+			double* jointRanges = 0;
+			double* restPoses = 0;
+			double* jointDamping = 0;
+			double* currentPositions = 0;
+
+			if (dofCount && (szLowerLimits == dofCount) && (szUpperLimits == dofCount) &&
+				(szJointRanges == dofCount) && (szRestPoses == dofCount))
 			{
-				lowerLimits[i] = pybullet_internalGetFloatFromSequence(lowerLimitsObj, i);
+				int szInBytes = sizeof(double) * dofCount;
-				upperLimits[i] = pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
+				int i;
-				jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i);
+				lowerLimits = (double*)malloc(szInBytes);
-				restPoses[i] = pybullet_internalGetFloatFromSequence(restPosesObj, i);
+				upperLimits = (double*)malloc(szInBytes);
+				jointRanges = (double*)malloc(szInBytes);
+				restPoses = (double*)malloc(szInBytes);
+
+				for (i = 0; i < dofCount; i++)
+				{
+					lowerLimits[i] = pybullet_internalGetFloatFromSequence(lowerLimitsObj, i);
+					upperLimits[i] = pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
+					jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i);
+					restPoses[i] = pybullet_internalGetFloatFromSequence(restPosesObj, i);
+				}
+				hasNullSpace = 1;
 			}
-			hasNullSpace = 1;
-		}
 
-		if (szCurrentPositions > 0)
+			if (szCurrentPositions > 0)
-		{
-			if (szCurrentPositions != dofCount)
 			{
-				PyErr_SetString(SpamError,
+				if (szCurrentPositions != dofCount)
-					"calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom.");
+				{
+					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
+				{
+					int szInBytes = sizeof(double) * szCurrentPositions;
+					int i;
+					currentPositions = (double*)malloc(szInBytes);
+					for (i = 0; i < szCurrentPositions; i++)
+					{
+						currentPositions[i] = pybullet_internalGetFloatFromSequence(currentPositionsObj, i);
+					}
+					hasCurrentPositions = 1;
+				}
+			}
+
+			if (szJointDamping > 0)
+			{
+				if (szJointDamping < dofCount)
+				{
+					printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, not using joint damping.");
+				}
+				else
+				{
+					int szInBytes = sizeof(double) * szJointDamping;
+					int i;
+					//if (szJointDamping != dofCount)
+					//{
+					//	printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, ignoring the additonal values.");
+					//}
+					jointDamping = (double*)malloc(szInBytes);
+					for (i = 0; i < szJointDamping; i++)
+					{
+						jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i);
+					}
+					hasJointDamping = 1;
+				}
+			}
+
+			if (hasPos)
+			{
+				b3SharedMemoryStatusHandle statusHandle;
+				int numPos = 0;
+				int resultBodyIndex;
+				int result;
+
+				b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
+				b3CalculateInverseKinematicsSelectSolver(command, solver);
+
+				if (hasCurrentPositions)
+				{
+					b3CalculateInverseKinematicsSetCurrentPositions(command, dofCount, currentPositions);
+				}
+				if (maxNumIterations > 0)
+				{
+					b3CalculateInverseKinematicsSetMaxNumIterations(command, maxNumIterations);
+				}
+				if (residualThreshold >= 0)
+				{
+					b3CalculateInverseKinematicsSetResidualThreshold(command, residualThreshold);
+				}
+
+				if (hasNullSpace)
+				{
+					if (hasOrn)
+					{
+						b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses);
+					}
+					else
+					{
+						b3CalculateInverseKinematicsPosWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses);
+					}
+				}
+				else
+				{
+					if (hasOrn)
+					{
+						b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, endEffectorLinkIndex, pos, ori);
+					}
+					else
+					{
+						//b3CalculateInverseKinematicsAddTargetPurePosition(command, endEffectorLinkIndex, pos);
+						b3CalculateInverseKinematicsAddTargetsPurePosition(command, numEndEffectorPositions, indices, positions);
+					}
+				}
+
+				if (hasJointDamping)
+				{
+					b3CalculateInverseKinematicsSetJointDamping(command, dofCount, jointDamping);
+				}
+				free(currentPositions);
+				free(jointDamping);
+
 				free(lowerLimits);
 				free(upperLimits);
 				free(jointRanges);
 				free(restPoses);
-				return NULL;
-			}
-			else
-			{
-				int szInBytes = sizeof(double) * szCurrentPositions;
-				int i;
-				currentPositions = (double*)malloc(szInBytes);
-				for (i = 0; i < szCurrentPositions; i++)
-				{
-					currentPositions[i] = pybullet_internalGetFloatFromSequence(currentPositionsObj, i);
-				}
-				hasCurrentPositions = 1;
-			}
-		}
 
-		if (szJointDamping > 0)
+				statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
-		{
-			if (szJointDamping < dofCount)
-			{
-				printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, not using joint damping.");
-			}
-			else
-			{
-				int szInBytes = sizeof(double) * szJointDamping;
-				int i;
-				//if (szJointDamping != dofCount)
-				//{
-				//	printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, ignoring the additonal values.");
-				//}
-				jointDamping = (double*)malloc(szInBytes);
-				for (i = 0; i < szJointDamping; i++)
-				{
-					jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i);
-				}
-				hasJointDamping = 1;
-			}
-		}
 
-		if (hasPos)
-		{
-			b3SharedMemoryStatusHandle statusHandle;
-			int numPos = 0;
-			int resultBodyIndex;
-			int result;
-
-			b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
-			b3CalculateInverseKinematicsSelectSolver(command, solver);
-
-			if (hasCurrentPositions)
-			{
-				b3CalculateInverseKinematicsSetCurrentPositions(command, dofCount, currentPositions);
-			}
-			if (maxNumIterations > 0)
-			{
-				b3CalculateInverseKinematicsSetMaxNumIterations(command, maxNumIterations);
-			}
-			if (residualThreshold >= 0)
-			{
-				b3CalculateInverseKinematicsSetResidualThreshold(command, residualThreshold);
-			}
-
-			if (hasNullSpace)
-			{
-				if (hasOrn)
-				{
-					b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses);
-				}
-				else
-				{
-					b3CalculateInverseKinematicsPosWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses);
-				}
-			}
-			else
-			{
-				if (hasOrn)
-				{
-					b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, endEffectorLinkIndex, pos, ori);
-				}
-				else
-				{
-					//b3CalculateInverseKinematicsAddTargetPurePosition(command, endEffectorLinkIndex, pos);
-					b3CalculateInverseKinematicsAddTargetsPurePosition(command, numEndEffectorPositions, indices, positions);
-				}
-			}
-
-			if (hasJointDamping)
-			{
-				b3CalculateInverseKinematicsSetJointDamping(command, dofCount, jointDamping);
-			}
-			free(currentPositions);
-			free(jointDamping);
-
-			free(lowerLimits);
-			free(upperLimits);
-			free(jointRanges);
-			free(restPoses);
-
-			statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
-
-			result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
-				&resultBodyIndex,
-				&numPos,
-				0);
-			if (result && numPos)
-			{
-				int i;
-				PyObject* pylist;
-				double* ikOutPutJointPos = (double*)malloc(numPos * sizeof(double));
 				result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
 					&resultBodyIndex,
 					&numPos,
-					ikOutPutJointPos);
+					0);
-				pylist = PyTuple_New(numPos);
+				if (result && numPos)
-				for (i = 0; i < numPos; i++)
 				{
-					PyTuple_SetItem(pylist, i,
+					int i;
-						PyFloat_FromDouble(ikOutPutJointPos[i]));
+					PyObject* pylist;
-				}
+					double* ikOutPutJointPos = (double*)malloc(numPos * sizeof(double));
+					result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+						&resultBodyIndex,
+						&numPos,
+						ikOutPutJointPos);
+					pylist = PyTuple_New(numPos);
+					for (i = 0; i < numPos; i++)
+					{
+						PyTuple_SetItem(pylist, i,
+							PyFloat_FromDouble(ikOutPutJointPos[i]));
+					}
 
-				free(ikOutPutJointPos);
+					free(ikOutPutJointPos);
-				return pylist;
+					return pylist;
+				}
+				else
+				{
+					PyErr_SetString(SpamError,
+						"Error in calculateInverseKinematics");
+					return NULL;
+				}
 			}
 			else
 			{
 				PyErr_SetString(SpamError,
-					"Error in calculateInverseKinematics");
+					"calculateInverseKinematics couldn't extract position vector3");
 				return NULL;
 			}
 		}
-		else
-		{
-			PyErr_SetString(SpamError,
-				"calculateInverseKinematics couldn't extract position vector3");
-			return NULL;
-		}
 	}
 
 	Py_INCREF(Py_None);