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

pybullet.calculateInverseKinematics: expose null space method with and without orientation

Browse Source 
add inverse_kinematics.py and hello_pybullet.py pybullet examples
add m_worldLinkFramePosition/Orientation fields to b3LinkState, and in pybullet.getLinkState (URDF link frame in Cartesian/world coordinates)
This commit is contained in:
Erwin Coumans
2017-01-11 21:39:22 -08:00
parent 3d6584962a
commit 4897139dad
5 changed files with 223 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
examples/pybullet/pybullet.c
View File

@@ -2020,6 +2020,8 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args,PyObject*
PyObject* pyLinkStateWorldOrientation;
PyObject* pyLinkStateLocalInertialPosition;
PyObject* pyLinkStateLocalInertialOrientation;
PyObject* pyLinkStateWorldLinkFramePosition;
PyObject* pyLinkStateWorldLinkFrameOrientation;
struct b3LinkState linkState;
@@ -2094,11 +2096,26 @@ b3PhysicsClientHandle sm = 0;
PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
}
pyLinkState = PyTuple_New(4);
pyLinkStateWorldLinkFramePosition = PyTuple_New(3);
for (i = 0; i < 3; ++i) {
PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i,
PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i]));
}
pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4);
for (i = 0; i < 4; ++i) {
PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i,
PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i]));
}
pyLinkState = PyTuple_New(6);
PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition );
PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation);
return pyLinkState;
}
@@ -4048,13 +4065,18 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
int bodyIndex;
int endEffectorLinkIndex;
PyObject* targetPosObj;
PyObject* targetOrnObj;
PyObject* targetPosObj=0;
PyObject* targetOrnObj=0;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOO|i", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&physicsClientId))
PyObject* lowerLimitsObj = 0;
PyObject* upperLimitsObj = 0;
PyObject* jointRangesObj = 0;
PyObject* restPosesObj = 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))
{
return NULL;
}
@@ -4067,8 +4089,49 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
{
double pos[3];
double ori[4]={0,1.0,0,0};
if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori))
int hasPos =pybullet_internalSetVectord(targetPosObj,pos);
int hasOrn = 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 numJoints = b3GetNumJoints(sm, bodyIndex);
int hasNullSpace = 0;
double* lowerLimits = 0;
double* upperLimits = 0;
double* jointRanges = 0;
double* restPoses = 0;
if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) &&
(szJointRanges == numJoints) && (szRestPoses == numJoints))
{
int szInBytes = sizeof(double) * numJoints;
int i;
PyObject* pylist = 0;
lowerLimits = (double*)malloc(szInBytes);
upperLimits = (double*)malloc(szInBytes);
jointRanges = (double*)malloc(szInBytes);
restPoses = (double*)malloc(szInBytes);
for (i = 0; i < numJoints; 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;
}
if (hasPos)
{
b3SharedMemoryStatusHandle statusHandle;
int numPos=0;
@@ -4076,7 +4139,27 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
int result;
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex);
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori);
if (hasNullSpace)
{
if (hasOrn)
{
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses);
} else
{
b3CalculateInverseKinematicsPosWithNullSpaceVel(command,numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses);
}
} else
{
if (hasOrn)
{
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,endEffectorLinkIndex,pos,ori);
} else
{
b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,