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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
Erwin Coumans
2017-09-24 09:49:53 -07:00
parent 63ee841bea 17709ae690
commit 70364445b8
7 changed files with 243 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

260
examples/pybullet/pybullet.c
View File

@@ -445,10 +445,10 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
{
printf("Connection terminated, couldn't get body info\n");
b3DisconnectSharedMemory(sm);
sm = 0;
sm = 0;
sPhysicsClients1[freeIndex] = 0;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
return PyInt_FromLong(-1);
}
}
@@ -2835,7 +2835,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
if (info.m_jointName)
{
PyTuple_SetItem(pyListJointInfo, 1,
PyString_FromString(info.m_jointName));
PyString_FromString(info.m_jointName));
} else
{
PyTuple_SetItem(pyListJointInfo, 1,
@@ -4754,21 +4754,21 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject*
static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPointPtr)
{
/*
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5 double m_positionOnAInWS[3];//contact point location on object A,
in world space coordinates
6 double m_positionOnBInWS[3];//contact point location on object
A, in world space coordinates
7 double m_contactNormalOnBInWS[3];//the separating contact
normal, pointing from object B towards object A
8 double m_contactDistance;//negative number is penetration, positive
is distance.
9 double m_normalForce;
*/
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5 double m_positionOnAInWS[3];//contact point location on object A,
in world space coordinates
6 double m_positionOnBInWS[3];//contact point location on object
A, in world space coordinates
7 double m_contactNormalOnBInWS[3];//the separating contact
normal, pointing from object B towards object A
8 double m_contactDistance;//negative number is penetration, positive
is distance.
9 double m_normalForce;
*/
int i;
@@ -5559,9 +5559,9 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
{
int bodyUniqueIdA = -1;
int bodyUniqueIdB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
int linkIndexA = -2;
int linkIndexB = -2;
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
@@ -5583,24 +5583,24 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
}
commandHandle = b3InitRequestContactPointInformation(sm);
if (bodyUniqueIdA>=0)
{
b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
}
if (bodyUniqueIdB>=0)
{
b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
}
if (bodyUniqueIdA>=0)
{
b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
}
if (bodyUniqueIdB>=0)
{
b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
}
if (linkIndexA>=-1)
{
b3SetContactFilterLinkA( commandHandle, linkIndexA);
}
if (linkIndexB >=-1)
{
b3SetContactFilterLinkB( commandHandle, linkIndexB);
}
if (linkIndexA>=-1)
{
b3SetContactFilterLinkA( commandHandle, linkIndexA);
}
if (linkIndexB >=-1)
{
b3SetContactFilterLinkB( commandHandle, linkIndexB);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
@@ -6574,7 +6574,7 @@ static PyObject* pybullet_invertTransform(PyObject* self,
PyErr_SetString(SpamError, "Invalid input: expected position [x,y,z] and orientation [x,y,z,w].");
return NULL;
}
/// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
@@ -6956,8 +6956,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
/// Given an object id, joint positions, joint velocities and joint
/// accelerations,
/// compute the joint forces using Inverse Dynamics
static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
PyObject* args, PyObject* keywds)
static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int bodyUniqueId;
@@ -6966,14 +6965,21 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
PyObject* objAccelerations;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyUniqueId", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "objPositions",
"objVelocities", "objAccelerations",
"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist,
&bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations,
&physicsClientId))
{
static char* kwlist2[] = {"bodyIndex", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL};
static char* kwlist2[] = {"bodyIndex", "objPositions",
"objVelocities", "objAccelerations",
"physicsClientId", NULL};
PyErr_Clear();
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2, &bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2,
&bodyUniqueId, &objPositionsQ, &objVelocitiesQdot,
&objAccelerations, &physicsClientId))
{
return NULL;
}
@@ -7072,6 +7078,149 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
return Py_None;
}
/// Given an object id, joint positions, joint velocities and joint
/// accelerations, compute the Jacobian
static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int bodyUniqueId;
int linkIndex;
PyObject* localPosition;
PyObject* objPositions;
PyObject* objVelocities;
PyObject* objAccelerations;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "localPosition",
"objPositions", "objVelocities",
"objAccelerations", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOOOO|i", kwlist,
&bodyUniqueId, &linkIndex, &localPosition, &objPositions,
&objVelocities, &objAccelerations, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int szLoPos = PySequence_Size(localPosition);
int szObPos = PySequence_Size(objPositions);
int szObVel = PySequence_Size(objVelocities);
int szObAcc = PySequence_Size(objAccelerations);
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
if (numJoints && (szLoPos == 3) && (szObPos == numJoints) &&
(szObVel == numJoints) && (szObAcc == numJoints))
{
int byteSizeJoints = sizeof(double) * numJoints;
int byteSizeVec3 = sizeof(double) * 3;
int i;
PyObject* pyResultList = PyTuple_New(2);
double* localPoint = (double*)malloc(byteSizeVec3);
double* jointPositions = (double*)malloc(byteSizeJoints);
double* jointVelocities = (double*)malloc(byteSizeJoints);
double* jointAccelerations = (double*)malloc(byteSizeJoints);
double* linearJacobian = (double*)malloc(3 * byteSizeJoints);
double* angularJacobian = (double*)malloc(3 * byteSizeJoints);
pybullet_internalSetVectord(localPosition, localPoint);
for (i = 0; i < numJoints; i++)
{
jointPositions[i] =
pybullet_internalGetFloatFromSequence(objPositions, i);
jointVelocities[i] =
pybullet_internalGetFloatFromSequence(objVelocities, i);
jointAccelerations[i] =
pybullet_internalGetFloatFromSequence(objAccelerations, i);
}
{
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle commandHandle =
b3CalculateJacobianCommandInit(sm, bodyUniqueId,
linkIndex, localPoint, jointPositions,
jointVelocities, jointAccelerations);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CALCULATED_JACOBIAN_COMPLETED)
{
int dofCount;
b3GetStatusJacobian(statusHandle, &dofCount, NULL, NULL);
if (dofCount)
{
int byteSizeDofCount = sizeof(double) * dofCount;
double* linearJacobian = (double*)malloc(3 * byteSizeDofCount);
double* angularJacobian = (double*)malloc(3 * byteSizeDofCount);
b3GetStatusJacobian(statusHandle,
NULL,
linearJacobian,
angularJacobian);
if (linearJacobian)
{
int r;
PyObject* pymat = PyTuple_New(3);
for (r = 0; r < 3; ++r) {
int c;
PyObject* pyrow = PyTuple_New(dofCount);
for (c = 0; c < dofCount; ++c) {
int element = r * dofCount + c;
PyTuple_SetItem(pyrow, c,
PyFloat_FromDouble(linearJacobian[element]));
}
PyTuple_SetItem(pymat, r, pyrow);
}
PyTuple_SetItem(pyResultList, 0, pymat);
}
if (angularJacobian)
{
int r;
PyObject* pymat = PyTuple_New(3);
for (r = 0; r < 3; ++r) {
int c;
PyObject* pyrow = PyTuple_New(dofCount);
for (c = 0; c < dofCount; ++c) {
int element = r * dofCount + c;
PyTuple_SetItem(pyrow, c,
PyFloat_FromDouble(linearJacobian[element]));
}
PyTuple_SetItem(pymat, r, pyrow);
}
PyTuple_SetItem(pyResultList, 1, pymat);
}
}
}
else
{
PyErr_SetString(SpamError,
"Internal error in calculateJacobian");
}
}
free(localPoint);
free(jointPositions);
free(jointVelocities);
free(jointAccelerations);
free(linearJacobian);
free(angularJacobian);
if (pyResultList) return pyResultList;
}
else
{
PyErr_SetString(SpamError,
"calculateJacobian [numJoints] needs to be "
"positive, [local position] needs to be of "
"size 3 and [joint positions], "
"[joint velocities], [joint accelerations] "
"need to match the number of joints.");
return NULL;
}
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyMethodDef SpamMethods[] = {
@@ -7111,6 +7260,8 @@ static PyMethodDef SpamMethods[] = {
"This is for experimental purposes, use at own risk, magic may or not happen"},
{"loadURDF", (PyCFunction)pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS,
"bodyUniqueId = loadURDF(fileName, basePosition=[0.,0.,0.], baseOrientation=[0.,0.,0.,1.], "
"useMaximalCoordinates=0, useFixedBase=0, flags=0, globalScaling=1.0, physicsClientId=0)\n"
"Create a multibody by loading a URDF file."},
{"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS | METH_KEYWORDS,
@@ -7359,6 +7510,19 @@ static PyMethodDef SpamMethods[] = {
"Given an object id, joint positions, joint velocities and joint "
"accelerations, compute the joint forces using Inverse Dynamics"},
{"calculateJacobian", (PyCFunction)pybullet_calculateJacobian, METH_VARARGS | METH_KEYWORDS,
"Compute the jacobian for a specified local position on a body and its kinematics.\n"
"Args:\n"
" bodyIndex - a scalar defining the unique object id.\n"
" linkIndex - a scalar identifying the link containing the local point.\n"
" localPosition - a list of [x, y, z] of the coordinates of the local point.\n"
" objPositions - a list of the joint positions.\n"
" objVelocities - a list of the joint velocities.\n"
" objAccelerations - a list of the joint accelerations.\n"
"Returns:\n"
" linearJacobian - a list of the partial linear velocities of the jacobian.\n"
" angularJacobian - a list of the partial angular velocities of the jacobian.\n"},
{"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics,
METH_VARARGS | METH_KEYWORDS,
"Inverse Kinematics bindings: Given an object id, "