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 | 316 +++++++++--------- 4 files changed, 200 insertions(+), 157 deletions(-) diff --git a/build3/premake4.lua b/build3/premake4.lua index b74f3f86ab..e1167b0aae 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 8caf7320cc..4f2a30b705 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 3939dfc858..bad84062b6 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 4364cbbab7..b5f5f9304d 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; + { + 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 numJoints = b3GetNumJoints(sm, bodyUniqueId); - int dofCount = b3ComputeDofCount(sm, bodyUniqueId); + 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 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; + int szCurrentPositions = currentPositionsObj ? PySequence_Size(currentPositionsObj) : 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); + 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); - 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); + 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."); - 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++) + if (szCurrentPositions != dofCount) { - currentPositions[i] = pybullet_internalGetFloatFromSequence(currentPositionsObj, i); + 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; } - 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 + if (szJointDamping > 0) { - 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++) + if (szJointDamping < dofCount) { - jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); + 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; } - 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) + if (hasPos) { - b3CalculateInverseKinematicsSetMaxNumIterations(command, maxNumIterations); - } - if (residualThreshold >= 0) - { - b3CalculateInverseKinematicsSetResidualThreshold(command, residualThreshold); - } + b3SharedMemoryStatusHandle statusHandle; + int numPos = 0; + int resultBodyIndex; + int result; - if (hasNullSpace) - { - if (hasOrn) + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId); + b3CalculateInverseKinematicsSelectSolver(command, solver); + + if (hasCurrentPositions) { - b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); + b3CalculateInverseKinematicsSetCurrentPositions(command, dofCount, currentPositions); } - else + if (maxNumIterations > 0) { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); + b3CalculateInverseKinematicsSetMaxNumIterations(command, maxNumIterations); } - } - else - { - if (hasOrn) + if (residualThreshold >= 0) { - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, endEffectorLinkIndex, pos, ori); + 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 { - //b3CalculateInverseKinematicsAddTargetPurePosition(command, endEffectorLinkIndex, pos); - b3CalculateInverseKinematicsAddTargetsPurePosition(command, numEndEffectorPositions, indices, positions); + 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); + if (hasJointDamping) + { + b3CalculateInverseKinematicsSetJointDamping(command, dofCount, jointDamping); + } + free(currentPositions); + free(jointDamping); - free(lowerLimits); - free(upperLimits); - free(jointRanges); - free(restPoses); + free(lowerLimits); + free(upperLimits); + free(jointRanges); + free(restPoses); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + 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) + { + 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; + } + else { - PyTuple_SetItem(pylist, i, - PyFloat_FromDouble(ikOutPutJointPos[i])); + PyErr_SetString(SpamError, + "Error in calculateInverseKinematics"); + return NULL; } - - free(ikOutPutJointPos); - return pylist; } 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);