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

28
examples/pybullet/pybullet.c
View File

@@ -4364,9 +4364,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* upperLimitsObj = 0;
PyObject* jointRangesObj = 0;
PyObject* restPosesObj = 0;
PyObject* jointDampingObj = 0;
static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOi", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId))
static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", "jointDamping", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOiO", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId, & jointDampingObj))
{
return NULL;
}
@@ -4386,18 +4387,21 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
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 numJoints = b3GetNumJoints(sm, bodyIndex);
int hasNullSpace = 0;
int hasJointDamping = 0;
double* lowerLimits = 0;
double* upperLimits = 0;
double* jointRanges = 0;
double* restPoses = 0;
double* jointDamping = 0;
if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) &&
(szJointRanges == numJoints) && (szRestPoses == numJoints))
(szJointRanges == numJoints) && (szRestPoses == numJoints))
{
int szInBytes = sizeof(double) * numJoints;
int i;
@@ -4414,11 +4418,22 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
jointRanges[i] =
pybullet_internalGetFloatFromSequence(jointRangesObj, i);
restPoses[i] =
restPoses[i] =
pybullet_internalGetFloatFromSequence(restPosesObj, i);
}
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)
{
@@ -4448,6 +4463,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos);
}
}
if (hasJointDamping)
{
b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);