Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Set joint damping in pybullet.

Browse Source
This commit is contained in:
yunfeibai
2017-02-03 12:03:07 -08:00
parent ce9378f819
commit ac5a8aa2d6
1 changed files with 24 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
examples/pybullet/pybullet.c
View File

@@ -4364,9 +4364,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* upperLimitsObj = 0; PyObject* upperLimitsObj = 0;
PyObject* jointRangesObj = 0; PyObject* jointRangesObj = 0;
PyObject* restPosesObj = 0; PyObject* restPosesObj = 0;
PyObject* jointDampingObj = 0;
static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", NULL }; static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", "jointDamping", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOi", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOiO", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId, & jointDampingObj))
{ {
return NULL; return NULL;
} }
@@ -4386,15 +4387,18 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
int szUpperLimits = upperLimitsObj? PySequence_Size(upperLimitsObj): 0; int szUpperLimits = upperLimitsObj? PySequence_Size(upperLimitsObj): 0;
int szJointRanges = jointRangesObj? PySequence_Size(jointRangesObj):0; int szJointRanges = jointRangesObj? PySequence_Size(jointRangesObj):0;
int szRestPoses = restPosesObj? PySequence_Size(restPosesObj):0; int szRestPoses = restPosesObj? PySequence_Size(restPosesObj):0;
int szJointDamping = jointDampingObj? PySequence_Size(jointDampingObj):0;
int numJoints = b3GetNumJoints(sm, bodyIndex); int numJoints = b3GetNumJoints(sm, bodyIndex);
int hasNullSpace = 0; int hasNullSpace = 0;
int hasJointDamping = 0;
double* lowerLimits = 0; double* lowerLimits = 0;
double* upperLimits = 0; double* upperLimits = 0;
double* jointRanges = 0; double* jointRanges = 0;
double* restPoses = 0; double* restPoses = 0;
double* jointDamping = 0;
if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) &&
(szJointRanges == numJoints) && (szRestPoses == numJoints)) (szJointRanges == numJoints) && (szRestPoses == numJoints))
@@ -4420,6 +4424,17 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
hasNullSpace = 1; hasNullSpace = 1;
} }
if (numJoints && (szJointDamping == numJoints))
{
int szInBytes = sizeof(double) * numJoints;
jointDamping = (double*)malloc(szInBytes);
for (int i = 0; i < numJoints; i++)
{
jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i);
}
hasJointDamping = 1;
}
if (hasPos) if (hasPos)
{ {
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
@@ -4449,6 +4464,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
} }
} }
if (hasJointDamping)
{
b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle, result = b3GetStatusInverseKinematicsJointPositions(statusHandle,