From 226819b839b768ac0000f8fa8bcda47d46e27b23 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 7 Aug 2019 21:57:05 -0700 Subject: [PATCH] premake add enable_stable_pd option, so Bullet can be compiled without C++11 (Visual Studio 2010 etc) PyBullet: improve sleeping: 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) Fix PyBullet compilation of Visual Studion 2010 --- build3/premake4.lua | 7 + .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 21 ++ examples/pybullet/premake4.lua | 13 +- examples/pybullet/pybullet.c | 354 +++++++++--------- 4 files changed, 219 insertions(+), 176 deletions(-) diff --git a/build3/premake4.lua b/build3/premake4.lua index b74f3f86a..e1167b0aa 100644 --- a/build3/premake4.lua +++ b/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", diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 8caf7320c..4f2a30b70 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/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) { diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 3939dfc85..bad84062b 100644 --- a/examples/pybullet/premake4.lua +++ b/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"} diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 4364cbbab..b5f5f9304 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4311,6 +4311,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)) @@ -4325,12 +4331,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) @@ -6061,6 +6062,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)) @@ -6072,7 +6074,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) @@ -10971,10 +10973,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) @@ -10982,197 +10986,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; - int i; - lowerLimits = (double*)malloc(szInBytes); - upperLimits = (double*)malloc(szInBytes); - jointRanges = (double*)malloc(szInBytes); - restPoses = (double*)malloc(szInBytes); + double pos[3] = { 0, 0, 0 }; + double ori[4] = { 0, 0, 0, 1 }; + int hasPos = numEndEffectorPositions > 0; + int hasOrn = 0;// pybullet_internalSetVector4d(targetOrnObj, ori); - 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); - upperLimits[i] = pybullet_internalGetFloatFromSequence(upperLimitsObj, i); - jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i); - restPoses[i] = pybullet_internalGetFloatFromSequence(restPosesObj, i); + int szInBytes = sizeof(double) * dofCount; + int i; + lowerLimits = (double*)malloc(szInBytes); + 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 != dofCount) + if (szCurrentPositions > 0) { - PyErr_SetString(SpamError, - "calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom."); + if (szCurrentPositions != dofCount) + { + 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) - { - 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; - } - } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - 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); - pylist = PyTuple_New(numPos); - for (i = 0; i < numPos; i++) + 0); + if (result && numPos) { - PyTuple_SetItem(pylist, i, - PyFloat_FromDouble(ikOutPutJointPos[i])); - } + int 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); - return pylist; + free(ikOutPutJointPos); + 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);