mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 06:00:12 +00:00
3913 lines
119 KiB
C
3913 lines
119 KiB
C
#include "../SharedMemory/PhysicsClientC_API.h"
|
|
#include "../SharedMemory/PhysicsDirectC_API.h"
|
|
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
|
|
#ifdef BT_ENABLE_ENET
|
|
#include "../SharedMemory/PhysicsClientUDP_C_API.h"
|
|
#endif //BT_ENABLE_ENET
|
|
|
|
#ifdef __APPLE__
|
|
#include <Python/Python.h>
|
|
#else
|
|
#include <Python.h>
|
|
#endif
|
|
|
|
#ifdef PYBULLET_USE_NUMPY
|
|
#include <numpy/arrayobject.h>
|
|
#endif
|
|
|
|
#if PY_MAJOR_VERSION >= 3
|
|
#define PyInt_FromLong PyLong_FromLong
|
|
#define PyString_FromString PyBytes_FromString
|
|
#endif
|
|
|
|
enum eCONNECT_METHOD {
|
|
eCONNECT_GUI = 1,
|
|
eCONNECT_DIRECT = 2,
|
|
eCONNECT_SHARED_MEMORY = 3,
|
|
eCONNECT_UDP = 4,
|
|
};
|
|
|
|
static PyObject* SpamError;
|
|
static b3PhysicsClientHandle sm = 0;
|
|
|
|
|
|
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
|
|
double v = 0.0;
|
|
PyObject* item;
|
|
|
|
if (PyList_Check(seq)) {
|
|
item = PyList_GET_ITEM(seq, index);
|
|
v = PyFloat_AsDouble(item);
|
|
}
|
|
else {
|
|
item = PyTuple_GET_ITEM(seq, index);
|
|
v = PyFloat_AsDouble(item);
|
|
}
|
|
return v;
|
|
}
|
|
|
|
|
|
// internal function to set a float matrix[16]
|
|
// used to initialize camera position with
|
|
// a view and projection matrix in renderImage()
|
|
//
|
|
// // Args:
|
|
// matrix - float[16] which will be set by values from objMat
|
|
static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
|
|
int i, len;
|
|
PyObject* seq;
|
|
|
|
seq = PySequence_Fast(objMat, "expected a sequence");
|
|
if (seq)
|
|
{
|
|
len = PySequence_Size(objMat);
|
|
if (len == 16) {
|
|
for (i = 0; i < len; i++) {
|
|
matrix[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
Py_DECREF(seq);
|
|
return 1;
|
|
}
|
|
Py_DECREF(seq);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
// internal function to set a float vector[3]
|
|
// used to initialize camera position with
|
|
// a view and projection matrix in renderImage()
|
|
//
|
|
// // Args:
|
|
// vector - float[3] which will be set by values from objMat
|
|
static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
|
|
int i, len;
|
|
PyObject* seq = 0;
|
|
if (objVec == NULL)
|
|
return 0;
|
|
|
|
seq = PySequence_Fast(objVec, "expected a sequence");
|
|
if (seq)
|
|
{
|
|
|
|
len = PySequence_Size(objVec);
|
|
if (len == 3) {
|
|
for (i = 0; i < len; i++) {
|
|
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
Py_DECREF(seq);
|
|
return 1;
|
|
}
|
|
Py_DECREF(seq);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
// vector - double[3] which will be set by values from obVec
|
|
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
|
|
int i, len;
|
|
PyObject* seq;
|
|
if (obVec == NULL)
|
|
return 0;
|
|
|
|
seq = PySequence_Fast(obVec, "expected a sequence");
|
|
if (seq)
|
|
{
|
|
len = PySequence_Size(obVec);
|
|
if (len == 3) {
|
|
for (i = 0; i < len; i++) {
|
|
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
Py_DECREF(seq);
|
|
return 1;
|
|
}
|
|
Py_DECREF(seq);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
// vector - double[3] which will be set by values from obVec
|
|
static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) {
|
|
int i, len;
|
|
PyObject* seq;
|
|
if (obVec == NULL)
|
|
return 0;
|
|
|
|
seq = PySequence_Fast(obVec, "expected a sequence");
|
|
len = PySequence_Size(obVec);
|
|
if (len == 4) {
|
|
for (i = 0; i < len; i++) {
|
|
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
Py_DECREF(seq);
|
|
return 1;
|
|
}
|
|
Py_DECREF(seq);
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// Step through one timestep of the simulation
|
|
static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) {
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
|
|
if (b3CanSubmitCommand(sm)) {
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(
|
|
sm, b3InitStepSimulationCommand(sm));
|
|
statusType = b3GetStatusType(statusHandle);
|
|
}
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) {
|
|
if (0 != sm) {
|
|
PyErr_SetString(SpamError,
|
|
"Already connected to physics server, disconnect first.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
int method = eCONNECT_GUI;
|
|
int key = SHARED_MEMORY_KEY;
|
|
int port = 1234;
|
|
const char* hostName = "localhost";
|
|
|
|
int size = PySequence_Size(args);
|
|
if (size == 1)
|
|
{
|
|
if (!PyArg_ParseTuple(args, "i", &method)) {
|
|
PyErr_SetString(SpamError,
|
|
"connectPhysicsServer expected argument GUI, "
|
|
"DIRECT, SHARED_MEMORY or UDP");
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
if (size == 2)
|
|
{
|
|
if (!PyArg_ParseTuple(args, "ii", &method, &key))
|
|
{
|
|
if (!PyArg_ParseTuple(args, "is", &method, &hostName))
|
|
{
|
|
PyErr_SetString(SpamError,
|
|
"connectPhysicsServer cannot parse second argument (either integer or string)");
|
|
return NULL;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
if (size == 3)
|
|
{
|
|
if (!PyArg_ParseTuple(args, "isi", &method, &hostName, &port))
|
|
{
|
|
PyErr_SetString(SpamError,
|
|
"connectPhysicsServer 3 arguments: method, hostname, port");
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
|
|
switch (method) {
|
|
case eCONNECT_GUI: {
|
|
int argc = 0;
|
|
char* argv[1] = {0};
|
|
|
|
#ifdef __APPLE__
|
|
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
|
|
#else
|
|
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
|
|
#endif
|
|
break;
|
|
}
|
|
case eCONNECT_DIRECT: {
|
|
sm = b3ConnectPhysicsDirect();
|
|
break;
|
|
}
|
|
case eCONNECT_SHARED_MEMORY: {
|
|
sm = b3ConnectSharedMemory(key);
|
|
break;
|
|
}
|
|
case eCONNECT_UDP:
|
|
{
|
|
#ifdef BT_ENABLE_ENET
|
|
|
|
sm = b3ConnectPhysicsUDP(hostName, port);
|
|
#else
|
|
PyErr_SetString(SpamError, "UDP is not enabled in this pybullet build");
|
|
return NULL;
|
|
#endif //BT_ENABLE_ENET
|
|
|
|
break;
|
|
}
|
|
|
|
default: {
|
|
PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument");
|
|
return NULL;
|
|
}
|
|
};
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
|
|
PyObject* args) {
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
{
|
|
b3DisconnectSharedMemory(sm);
|
|
sm = 0;
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) {
|
|
int size = PySequence_Size(args);
|
|
const char* worldFileName = "";
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (size == 1) {
|
|
if (!PyArg_ParseTuple(args, "s", &worldFileName))
|
|
{
|
|
return NULL;
|
|
}
|
|
else
|
|
{
|
|
b3SharedMemoryCommandHandle command;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
|
|
command = b3SaveWorldCommandInit(sm, worldFileName);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType != CMD_SAVE_WORLD_COMPLETED) {
|
|
PyErr_SetString(SpamError, "saveWorld command execution failed.");
|
|
return NULL;
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
PyErr_SetString(SpamError, "Cannot execute saveWorld command.");
|
|
return NULL;
|
|
|
|
}
|
|
|
|
#define MAX_SDF_BODIES 512
|
|
static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
const char* bulletFileName = "";
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
b3SharedMemoryCommandHandle command;
|
|
int i,numBodies;
|
|
int bodyIndicesOut[MAX_SDF_BODIES];
|
|
PyObject* pylist = 0;
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
if (size == 1) {
|
|
if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL;
|
|
}
|
|
|
|
command = b3LoadBulletCommandInit(sm, bulletFileName);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType != CMD_BULLET_LOADING_COMPLETED)
|
|
{
|
|
PyErr_SetString(SpamError, "Couldn't load .bullet file.");
|
|
return NULL;
|
|
}
|
|
|
|
numBodies =
|
|
b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
|
|
if (numBodies > MAX_SDF_BODIES) {
|
|
PyErr_SetString(SpamError, "loadBullet exceeds body capacity");
|
|
return NULL;
|
|
}
|
|
|
|
pylist = PyTuple_New(numBodies);
|
|
|
|
if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) {
|
|
for (i = 0; i < numBodies; i++) {
|
|
PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i]));
|
|
}
|
|
}
|
|
return pylist;
|
|
|
|
|
|
}
|
|
|
|
static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
const char* bulletFileName = "";
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
b3SharedMemoryCommandHandle command;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
if (size == 1) {
|
|
if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL;
|
|
}
|
|
command = b3SaveBulletCommandInit(sm, bulletFileName);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType != CMD_BULLET_SAVING_COMPLETED)
|
|
{
|
|
PyErr_SetString(SpamError, "Couldn't save .bullet file.");
|
|
return NULL;
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
|
|
static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
const char* mjcfjFileName = "";
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
b3SharedMemoryCommandHandle command;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
if (size == 1) {
|
|
if (!PyArg_ParseTuple(args, "s", &mjcfjFileName)) return NULL;
|
|
}
|
|
command = b3LoadMJCFCommandInit(sm, mjcfjFileName);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType != CMD_MJCF_LOADING_COMPLETED)
|
|
{
|
|
PyErr_SetString(SpamError, "Couldn't load .mjcf file.");
|
|
return NULL;
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
double fixedTimeStep = -1;
|
|
int numSolverIterations = -1;
|
|
int useSplitImpulse = -1;
|
|
double splitImpulsePenetrationThreshold = -1;
|
|
int numSubSteps = -1;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps", NULL };
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps))
|
|
{
|
|
return NULL;
|
|
}
|
|
{
|
|
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
|
|
if (numSolverIterations >= 0)
|
|
{
|
|
b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
|
|
}
|
|
if (numSubSteps >= 0)
|
|
{
|
|
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
|
|
}
|
|
if (fixedTimeStep >= 0)
|
|
{
|
|
b3PhysicsParamSetTimeStep(command, fixedTimeStep);
|
|
}
|
|
if (useSplitImpulse >= 0)
|
|
{
|
|
b3PhysicsParamSetUseSplitImpulse(command,useSplitImpulse);
|
|
}
|
|
if (splitImpulsePenetrationThreshold >= 0)
|
|
{
|
|
b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold);
|
|
}
|
|
|
|
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
}
|
|
#if 0
|
|
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
|
|
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx, double gravy, double gravz);
|
|
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
|
|
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
|
|
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
|
|
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
|
|
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
|
|
#endif
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
// Load a robot from a URDF file (universal robot description format)
|
|
// function can be called without arguments and will default
|
|
// to position (0,0,1) with orientation(0,0,0,1)
|
|
// els(x,y,z) or
|
|
// loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w)
|
|
static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
static char *kwlist[] = { "fileName", "basePosition", "baseOrientation", "useMaximalCoordinates","useFixedBase", NULL };
|
|
|
|
static char *kwlistBackwardCompatible4[] = { "fileName", "startPosX", "startPosY", "startPosZ", NULL };
|
|
static char *kwlistBackwardCompatible8[] = { "fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY","startOrnZ","startOrnW", NULL };
|
|
|
|
|
|
int bodyIndex = -1;
|
|
const char* urdfFileName = "";
|
|
|
|
double startPosX = 0.0;
|
|
double startPosY = 0.0;
|
|
double startPosZ = 0.0;
|
|
double startOrnX = 0.0;
|
|
double startOrnY = 0.0;
|
|
double startOrnZ = 0.0;
|
|
double startOrnW = 1.0;
|
|
int useMaximalCoordinates = 0;
|
|
int useFixedBase = 0;
|
|
|
|
int backwardsCompatibilityArgs = 0;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (PyArg_ParseTupleAndKeywords(args, keywds, "sddd", kwlistBackwardCompatible4, &urdfFileName, &startPosX,
|
|
&startPosY, &startPosZ))
|
|
{
|
|
backwardsCompatibilityArgs = 1;
|
|
}
|
|
else
|
|
{
|
|
PyErr_Clear();
|
|
}
|
|
|
|
|
|
if (PyArg_ParseTupleAndKeywords(args, keywds, "sddddddd", kwlistBackwardCompatible8,&urdfFileName, &startPosX,
|
|
&startPosY, &startPosZ, &startOrnX, &startOrnY,&startOrnZ, &startOrnW))
|
|
{
|
|
backwardsCompatibilityArgs = 1;
|
|
}
|
|
else
|
|
{
|
|
PyErr_Clear();
|
|
}
|
|
|
|
|
|
|
|
if (!backwardsCompatibilityArgs)
|
|
{
|
|
PyObject* basePosObj = 0;
|
|
PyObject* baseOrnObj = 0;
|
|
double basePos[3];
|
|
double baseOrn[4];
|
|
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates,&useFixedBase))
|
|
{
|
|
|
|
return NULL;
|
|
}
|
|
else
|
|
{
|
|
if (basePosObj)
|
|
{
|
|
if (!pybullet_internalSetVectord(basePosObj, basePos))
|
|
{
|
|
PyErr_SetString(SpamError, "Cannot convert basePosition.");
|
|
return NULL;
|
|
}
|
|
startPosX = basePos[0];
|
|
startPosY = basePos[1];
|
|
startPosZ = basePos[2];
|
|
|
|
}
|
|
if (baseOrnObj)
|
|
{
|
|
if (!pybullet_internalSetVector4d(baseOrnObj, baseOrn))
|
|
{
|
|
PyErr_SetString(SpamError, "Cannot convert baseOrientation.");
|
|
return NULL;
|
|
}
|
|
startOrnX = baseOrn[0];
|
|
startOrnY = baseOrn[1];
|
|
startOrnZ = baseOrn[2];
|
|
startOrnW = baseOrn[3];
|
|
}
|
|
}
|
|
}
|
|
|
|
if (strlen(urdfFileName)) {
|
|
// printf("(%f, %f, %f) (%f, %f, %f, %f)\n",
|
|
// startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW);
|
|
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
b3SharedMemoryCommandHandle command =
|
|
b3LoadUrdfCommandInit(sm, urdfFileName);
|
|
|
|
// setting the initial position, orientation and other arguments are
|
|
// optional
|
|
b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ);
|
|
b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,
|
|
startOrnZ, startOrnW);
|
|
if (useMaximalCoordinates)
|
|
{
|
|
b3LoadUrdfCommandSetUseMultiBody(command, 0);
|
|
}
|
|
if (useFixedBase)
|
|
{
|
|
b3LoadUrdfCommandSetUseFixedBase(command, 1);
|
|
}
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType != CMD_URDF_LOADING_COMPLETED) {
|
|
PyErr_SetString(SpamError, "Cannot load URDF file.");
|
|
return NULL;
|
|
}
|
|
bodyIndex = b3GetStatusBodyIndex(statusHandle);
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"Empty filename, method expects 1, 4 or 8 arguments.");
|
|
return NULL;
|
|
}
|
|
return PyLong_FromLong(bodyIndex);
|
|
}
|
|
|
|
|
|
|
|
|
|
static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) {
|
|
const char* sdfFileName = "";
|
|
int size = PySequence_Size(args);
|
|
int numBodies = 0;
|
|
int i;
|
|
int bodyIndicesOut[MAX_SDF_BODIES];
|
|
PyObject* pylist = 0;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (size == 1) {
|
|
if (!PyArg_ParseTuple(args, "s", &sdfFileName)) return NULL;
|
|
}
|
|
|
|
commandHandle = b3LoadSdfCommandInit(sm, sdfFileName);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType != CMD_SDF_LOADING_COMPLETED) {
|
|
PyErr_SetString(SpamError, "Cannot load SDF file.");
|
|
return NULL;
|
|
}
|
|
|
|
numBodies =
|
|
b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
|
|
if (numBodies > MAX_SDF_BODIES) {
|
|
PyErr_SetString(SpamError, "SDF exceeds body capacity");
|
|
return NULL;
|
|
}
|
|
|
|
pylist = PyTuple_New(numBodies);
|
|
|
|
if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) {
|
|
for (i = 0; i < numBodies; i++) {
|
|
PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i]));
|
|
}
|
|
}
|
|
return pylist;
|
|
}
|
|
|
|
// Reset the simulation to remove all loaded objects
|
|
static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args) {
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(
|
|
sm, b3InitResetSimulationCommand(sm));
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) {
|
|
int size;
|
|
int bodyIndex, jointIndex, controlMode;
|
|
|
|
double targetPosition = 0.0;
|
|
double targetVelocity = 0.0;
|
|
double maxForce = 100000.0;
|
|
double appliedForce = 0.0;
|
|
double kp = 0.1;
|
|
double kd = 1.0;
|
|
int valid = 0;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
size = PySequence_Size(args);
|
|
if (size == 4) {
|
|
double targetValue = 0.0;
|
|
// see switch statement below for convertsions dependent on controlMode
|
|
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode,
|
|
&targetValue)) {
|
|
PyErr_SetString(SpamError, "Error parsing arguments");
|
|
return NULL;
|
|
}
|
|
valid = 1;
|
|
switch (controlMode) {
|
|
case CONTROL_MODE_POSITION_VELOCITY_PD: {
|
|
targetPosition = targetValue;
|
|
break;
|
|
}
|
|
case CONTROL_MODE_VELOCITY: {
|
|
targetVelocity = targetValue;
|
|
break;
|
|
}
|
|
case CONTROL_MODE_TORQUE: {
|
|
appliedForce = targetValue;
|
|
break;
|
|
}
|
|
default: { valid = 0; }
|
|
}
|
|
}
|
|
if (size == 5) {
|
|
double targetValue = 0.0;
|
|
// See switch statement for conversions
|
|
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode,
|
|
&targetValue, &maxForce)) {
|
|
PyErr_SetString(SpamError, "Error parsing arguments");
|
|
return NULL;
|
|
}
|
|
valid = 1;
|
|
|
|
switch (controlMode) {
|
|
case CONTROL_MODE_POSITION_VELOCITY_PD: {
|
|
targetPosition = targetValue;
|
|
break;
|
|
}
|
|
case CONTROL_MODE_VELOCITY: {
|
|
targetVelocity = targetValue;
|
|
break;
|
|
}
|
|
case CONTROL_MODE_TORQUE: {
|
|
valid = 0;
|
|
break;
|
|
}
|
|
default: { valid = 0; }
|
|
}
|
|
}
|
|
if (size == 6) {
|
|
double gain = 0.0;
|
|
double targetValue = 0.0;
|
|
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode,
|
|
&targetValue, &maxForce, &gain)) {
|
|
PyErr_SetString(SpamError, "Error parsing arguments");
|
|
return NULL;
|
|
}
|
|
valid = 1;
|
|
|
|
switch (controlMode) {
|
|
case CONTROL_MODE_POSITION_VELOCITY_PD: {
|
|
targetPosition = targetValue;
|
|
kp = gain;
|
|
break;
|
|
}
|
|
case CONTROL_MODE_VELOCITY: {
|
|
targetVelocity = targetValue;
|
|
kd = gain;
|
|
break;
|
|
}
|
|
case CONTROL_MODE_TORQUE: {
|
|
valid = 0;
|
|
break;
|
|
}
|
|
default: { valid = 0; }
|
|
}
|
|
}
|
|
if (size == 8) {
|
|
// only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
|
|
if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex,
|
|
&controlMode, &targetPosition, &targetVelocity,
|
|
&maxForce, &kp, &kd)) {
|
|
PyErr_SetString(SpamError, "Error parsing arguments");
|
|
return NULL;
|
|
}
|
|
valid = 1;
|
|
}
|
|
|
|
if (valid) {
|
|
int numJoints;
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
struct b3JointInfo info;
|
|
|
|
numJoints = b3GetNumJoints(sm, bodyIndex);
|
|
if ((jointIndex >= numJoints) || (jointIndex < 0)) {
|
|
PyErr_SetString(SpamError, "Joint index out-of-range.");
|
|
return NULL;
|
|
}
|
|
|
|
if ((controlMode != CONTROL_MODE_VELOCITY) &&
|
|
(controlMode != CONTROL_MODE_TORQUE) &&
|
|
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) {
|
|
PyErr_SetString(SpamError, "Illegral control mode.");
|
|
return NULL;
|
|
}
|
|
|
|
commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode);
|
|
|
|
b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
|
|
|
|
switch (controlMode) {
|
|
case CONTROL_MODE_VELOCITY: {
|
|
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,
|
|
targetVelocity);
|
|
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
|
|
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
|
|
break;
|
|
}
|
|
|
|
case CONTROL_MODE_TORQUE: {
|
|
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex,
|
|
appliedForce);
|
|
break;
|
|
}
|
|
|
|
case CONTROL_MODE_POSITION_VELOCITY_PD: {
|
|
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex,
|
|
targetPosition);
|
|
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
|
|
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,
|
|
targetVelocity);
|
|
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
|
|
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
|
|
break;
|
|
}
|
|
default: {}
|
|
};
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
|
|
return NULL;
|
|
}
|
|
|
|
static PyObject* pybullet_setRealTimeSimulation(PyObject* self,
|
|
PyObject* args) {
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
int enableRealTimeSimulation = 0;
|
|
int ret;
|
|
|
|
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
|
|
if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) {
|
|
PyErr_SetString(
|
|
SpamError,
|
|
"setRealTimeSimulation expected a single value (integer).");
|
|
return NULL;
|
|
}
|
|
ret =
|
|
b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
// ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
|
|
static PyObject* pybullet_setInternalSimFlags(PyObject* self,
|
|
PyObject* args) {
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
int enableRealTimeSimulation = 0;
|
|
int ret;
|
|
|
|
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
|
|
if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) {
|
|
PyErr_SetString(
|
|
SpamError,
|
|
"setInternalSimFlags expected a single value (integer).");
|
|
return NULL;
|
|
}
|
|
ret =
|
|
b3PhysicsParamSetInternalSimFlags(command, enableRealTimeSimulation);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
// ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
// Set the gravity of the world with (x, y, z) arguments
|
|
static PyObject* pybullet_setGravity(PyObject* self, PyObject* args) {
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
double gravX = 0.0;
|
|
double gravY = 0.0;
|
|
double gravZ = -10.0;
|
|
int ret;
|
|
|
|
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
|
|
if (!PyArg_ParseTuple(args, "ddd", &gravX, &gravY, &gravZ)) {
|
|
PyErr_SetString(SpamError, "setGravity expected (x,y,z) values.");
|
|
return NULL;
|
|
}
|
|
ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ);
|
|
// ret = b3PhysicsParamSetTimeStep(command, timeStep);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
// ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args) {
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
double timeStep = 0.001;
|
|
int ret;
|
|
|
|
|
|
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
|
|
if (!PyArg_ParseTuple(args, "d", &timeStep)) {
|
|
PyErr_SetString(SpamError,
|
|
"setTimeStep expected a single value (double).");
|
|
return NULL;
|
|
}
|
|
ret = b3PhysicsParamSetTimeStep(command, timeStep);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
// ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject *
|
|
pybullet_setDefaultContactERP(PyObject* self, PyObject* args)
|
|
{
|
|
if (0==sm)
|
|
{
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
double defaultContactERP=0.005;
|
|
int ret;
|
|
|
|
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
|
|
if (!PyArg_ParseTuple(args, "d", &defaultContactERP))
|
|
{
|
|
PyErr_SetString(SpamError, "default Contact ERP expected a single value (double).");
|
|
return NULL;
|
|
}
|
|
ret = b3PhysicsParamSetDefaultContactERP(command, defaultContactERP);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static int pybullet_internalGetBaseVelocity(
|
|
int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3]) {
|
|
baseLinearVelocity[0] = 0.;
|
|
baseLinearVelocity[1] = 0.;
|
|
baseLinearVelocity[2] = 0.;
|
|
|
|
baseAngularVelocity[0] = 0.;
|
|
baseAngularVelocity[1] = 0.;
|
|
baseAngularVelocity[2] = 0.;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return 0;
|
|
}
|
|
|
|
{
|
|
{
|
|
b3SharedMemoryCommandHandle cmd_handle =
|
|
b3RequestActualStateCommandInit(sm, bodyIndex);
|
|
b3SharedMemoryStatusHandle status_handle =
|
|
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
|
|
|
|
const int status_type = b3GetStatusType(status_handle);
|
|
const double* actualStateQdot;
|
|
// const double* jointReactionForces[];
|
|
|
|
|
|
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
|
|
PyErr_SetString(SpamError, "getBaseVelocity failed.");
|
|
return 0;
|
|
}
|
|
|
|
b3GetStatusActualState(
|
|
status_handle, 0 /* body_unique_id */,
|
|
0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
|
|
0 /*root_local_inertial_frame*/, 0,
|
|
&actualStateQdot, 0 /* joint_reaction_forces */);
|
|
|
|
// printf("joint reaction forces=");
|
|
// for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) {
|
|
// printf("%f ", jointReactionForces[i]);
|
|
// }
|
|
// now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2]
|
|
// and orientation x,y,z,w =
|
|
// actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6]
|
|
baseLinearVelocity[0] = actualStateQdot[0];
|
|
baseLinearVelocity[1] = actualStateQdot[1];
|
|
baseLinearVelocity[2] = actualStateQdot[2];
|
|
|
|
baseAngularVelocity[0] = actualStateQdot[3];
|
|
baseAngularVelocity[1] = actualStateQdot[4];
|
|
baseAngularVelocity[2] = actualStateQdot[5];
|
|
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
// Internal function used to get the base position and orientation
|
|
// Orientation is returned in quaternions
|
|
static int pybullet_internalGetBasePositionAndOrientation(
|
|
int bodyIndex, double basePosition[3], double baseOrientation[4]) {
|
|
basePosition[0] = 0.;
|
|
basePosition[1] = 0.;
|
|
basePosition[2] = 0.;
|
|
|
|
baseOrientation[0] = 0.;
|
|
baseOrientation[1] = 0.;
|
|
baseOrientation[2] = 0.;
|
|
baseOrientation[3] = 1.;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return 0;
|
|
}
|
|
|
|
{
|
|
{
|
|
b3SharedMemoryCommandHandle cmd_handle =
|
|
b3RequestActualStateCommandInit(sm, bodyIndex);
|
|
b3SharedMemoryStatusHandle status_handle =
|
|
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
|
|
|
|
const int status_type = b3GetStatusType(status_handle);
|
|
const double* actualStateQ;
|
|
// const double* jointReactionForces[];
|
|
|
|
|
|
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
|
|
PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
|
|
return 0;
|
|
}
|
|
|
|
b3GetStatusActualState(
|
|
status_handle, 0 /* body_unique_id */,
|
|
0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
|
|
0 /*root_local_inertial_frame*/, &actualStateQ,
|
|
0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */);
|
|
|
|
// printf("joint reaction forces=");
|
|
// for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) {
|
|
// printf("%f ", jointReactionForces[i]);
|
|
// }
|
|
// now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2]
|
|
// and orientation x,y,z,w =
|
|
// actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6]
|
|
basePosition[0] = actualStateQ[0];
|
|
basePosition[1] = actualStateQ[1];
|
|
basePosition[2] = actualStateQ[2];
|
|
|
|
baseOrientation[0] = actualStateQ[3];
|
|
baseOrientation[1] = actualStateQ[4];
|
|
baseOrientation[2] = actualStateQ[5];
|
|
baseOrientation[3] = actualStateQ[6];
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
// Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion
|
|
// values for the base link of your object
|
|
// Object is retrieved based on body index, which is the order
|
|
// the object was loaded into the simulation (0-based)
|
|
static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
|
|
PyObject* args) {
|
|
int bodyIndex = -1;
|
|
double basePosition[3];
|
|
double baseOrientation[4];
|
|
PyObject* pylistPos;
|
|
PyObject* pylistOrientation;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
|
|
PyErr_SetString(SpamError, "Expected a body index (integer).");
|
|
return NULL;
|
|
}
|
|
|
|
if (0 == pybullet_internalGetBasePositionAndOrientation(
|
|
bodyIndex, basePosition, baseOrientation)) {
|
|
PyErr_SetString(SpamError,
|
|
"GetBasePositionAndOrientation failed.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
PyObject* item;
|
|
int i;
|
|
int num = 3;
|
|
pylistPos = PyTuple_New(num);
|
|
for (i = 0; i < num; i++) {
|
|
item = PyFloat_FromDouble(basePosition[i]);
|
|
PyTuple_SetItem(pylistPos, i, item);
|
|
}
|
|
}
|
|
|
|
{
|
|
PyObject* item;
|
|
int i;
|
|
int num = 4;
|
|
pylistOrientation = PyTuple_New(num);
|
|
for (i = 0; i < num; i++) {
|
|
item = PyFloat_FromDouble(baseOrientation[i]);
|
|
PyTuple_SetItem(pylistOrientation, i, item);
|
|
}
|
|
}
|
|
|
|
{
|
|
PyObject* pylist;
|
|
pylist = PyTuple_New(2);
|
|
PyTuple_SetItem(pylist, 0, pylistPos);
|
|
PyTuple_SetItem(pylist, 1, pylistOrientation);
|
|
return pylist;
|
|
}
|
|
}
|
|
|
|
|
|
static PyObject* pybullet_getBaseVelocity(PyObject* self,
|
|
PyObject* args) {
|
|
int bodyIndex = -1;
|
|
double baseLinearVelocity[3];
|
|
double baseAngularVelocity[3];
|
|
PyObject* pylistLinVel=0;
|
|
PyObject* pylistAngVel=0;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
|
|
PyErr_SetString(SpamError, "Expected a body index (integer).");
|
|
return NULL;
|
|
}
|
|
|
|
if (0 == pybullet_internalGetBaseVelocity(
|
|
bodyIndex, baseLinearVelocity, baseAngularVelocity)) {
|
|
PyErr_SetString(SpamError,
|
|
"getBaseVelocity failed.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
PyObject* item;
|
|
int i;
|
|
int num = 3;
|
|
pylistLinVel = PyTuple_New(num);
|
|
for (i = 0; i < num; i++) {
|
|
item = PyFloat_FromDouble(baseLinearVelocity[i]);
|
|
PyTuple_SetItem(pylistLinVel, i, item);
|
|
}
|
|
}
|
|
|
|
{
|
|
PyObject* item;
|
|
int i;
|
|
int num = 3;
|
|
pylistAngVel = PyTuple_New(num);
|
|
for (i = 0; i < num; i++) {
|
|
item = PyFloat_FromDouble(baseAngularVelocity[i]);
|
|
PyTuple_SetItem(pylistAngVel, i, item);
|
|
}
|
|
}
|
|
|
|
{
|
|
PyObject* pylist;
|
|
pylist = PyTuple_New(2);
|
|
PyTuple_SetItem(pylist, 0, pylistLinVel);
|
|
PyTuple_SetItem(pylist, 1, pylistAngVel);
|
|
return pylist;
|
|
}
|
|
}
|
|
static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args)
|
|
{
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
int numBodies = b3GetNumBodies(sm);
|
|
|
|
#if PY_MAJOR_VERSION >= 3
|
|
return PyLong_FromLong(numBodies);
|
|
#else
|
|
return PyInt_FromLong(numBodies);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args)
|
|
{
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
int serialIndex = -1;
|
|
int bodyUniqueId = -1;
|
|
if (!PyArg_ParseTuple(args, "i", &serialIndex)) {
|
|
PyErr_SetString(SpamError, "Expected a serialIndex in range [0..number of bodies).");
|
|
return NULL;
|
|
}
|
|
bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex);
|
|
|
|
#if PY_MAJOR_VERSION >= 3
|
|
return PyLong_FromLong(bodyUniqueId);
|
|
#else
|
|
return PyInt_FromLong(bodyUniqueId);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args)
|
|
{
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
int bodyUniqueId= -1;
|
|
int numJoints = 0;
|
|
if (!PyArg_ParseTuple(args, "i", &bodyUniqueId))
|
|
{
|
|
PyErr_SetString(SpamError, "Expected a body unique id (integer).");
|
|
return NULL;
|
|
}
|
|
{
|
|
struct b3BodyInfo info;
|
|
if (b3GetBodyInfo(sm,bodyUniqueId,&info))
|
|
{
|
|
PyObject* pyListJointInfo = PyTuple_New(1);
|
|
PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName));
|
|
return pyListJointInfo;
|
|
} else
|
|
{
|
|
PyErr_SetString(SpamError, "Couldn't get body info");
|
|
return NULL;
|
|
}
|
|
}
|
|
}
|
|
|
|
PyErr_SetString(SpamError, "error in getBodyInfo.");
|
|
return NULL;
|
|
}
|
|
|
|
|
|
// Return the number of joints in an object based on
|
|
// body index; body index is based on order of sequence
|
|
// the object is loaded into simulation
|
|
static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args)
|
|
{
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
int bodyIndex = -1;
|
|
int numJoints = 0;
|
|
if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
|
|
PyErr_SetString(SpamError, "Expected a body index (integer).");
|
|
return NULL;
|
|
}
|
|
numJoints = b3GetNumJoints(sm, bodyIndex);
|
|
|
|
#if PY_MAJOR_VERSION >= 3
|
|
return PyLong_FromLong(numJoints);
|
|
#else
|
|
return PyInt_FromLong(numJoints);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
// Initalize all joint positions given a list of values
|
|
static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) {
|
|
int size;
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
size = PySequence_Size(args);
|
|
|
|
if (size == 3) {
|
|
int bodyIndex;
|
|
int jointIndex;
|
|
double targetValue;
|
|
|
|
if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) {
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int numJoints;
|
|
|
|
numJoints = b3GetNumJoints(sm, bodyIndex);
|
|
if ((jointIndex >= numJoints) || (jointIndex < 0)) {
|
|
PyErr_SetString(SpamError, "Joint index out-of-range.");
|
|
return NULL;
|
|
}
|
|
|
|
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
|
|
|
|
b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex,
|
|
targetValue);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
}
|
|
PyErr_SetString(SpamError, "error in resetJointState.");
|
|
return NULL;
|
|
}
|
|
|
|
|
|
|
|
|
|
static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity", NULL };
|
|
|
|
if (0 == sm)
|
|
{
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
int bodyIndex=0;
|
|
PyObject* linVelObj=0;
|
|
PyObject* angVelObj=0;
|
|
double linVel[3] = { 0, 0, 0 };
|
|
double angVel[3] = { 0, 0, 0 };
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OO", kwlist, &bodyIndex, &linVelObj, &angVelObj))
|
|
{
|
|
return NULL;
|
|
}
|
|
if (linVelObj || angVelObj)
|
|
{
|
|
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
|
|
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
|
|
|
|
if (linVelObj)
|
|
{
|
|
pybullet_internalSetVectord(linVelObj, linVel);
|
|
b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVel);
|
|
}
|
|
|
|
if (angVelObj)
|
|
{
|
|
pybullet_internalSetVectord(angVelObj, angVel);
|
|
b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVel);
|
|
}
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
else
|
|
{
|
|
PyErr_SetString(SpamError, "expected at least linearVelocity and/or angularVelocity.");
|
|
return NULL;
|
|
}
|
|
}
|
|
PyErr_SetString(SpamError, "error in resetJointState.");
|
|
return NULL;
|
|
}
|
|
|
|
|
|
|
|
// Reset the position and orientation of the base/root link, position [x,y,z]
|
|
// and orientation quaternion [x,y,z,w]
|
|
static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
|
|
PyObject* args) {
|
|
int size;
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
size = PySequence_Size(args);
|
|
|
|
if (size == 3) {
|
|
int bodyIndex;
|
|
PyObject* posObj;
|
|
PyObject* ornObj;
|
|
double pos[3];
|
|
double orn[4]; // as a quaternion
|
|
|
|
if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) {
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
|
|
{
|
|
PyObject* seq;
|
|
int len, i;
|
|
seq = PySequence_Fast(posObj, "expected a sequence");
|
|
len = PySequence_Size(posObj);
|
|
if (len == 3) {
|
|
for (i = 0; i < 3; i++) {
|
|
pos[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
} else {
|
|
PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
|
|
Py_DECREF(seq);
|
|
return NULL;
|
|
}
|
|
Py_DECREF(seq);
|
|
}
|
|
|
|
{
|
|
PyObject* seq;
|
|
int len, i;
|
|
seq = PySequence_Fast(ornObj, "expected a sequence");
|
|
len = PySequence_Size(ornObj);
|
|
if (len == 4) {
|
|
for (i = 0; i < 4; i++) {
|
|
orn[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
} else {
|
|
PyErr_SetString(
|
|
SpamError,
|
|
"orientation needs a 4 coordinates, quaternion [x,y,z,w].");
|
|
Py_DECREF(seq);
|
|
return NULL;
|
|
}
|
|
Py_DECREF(seq);
|
|
}
|
|
|
|
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
|
|
|
|
b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]);
|
|
b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1],
|
|
orn[2], orn[3]);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
}
|
|
PyErr_SetString(SpamError, "error in resetJointState.");
|
|
return NULL;
|
|
}
|
|
|
|
// Get the a single joint info for a specific bodyIndex
|
|
//
|
|
// Args:
|
|
// bodyIndex - integer indicating body in simulation
|
|
// jointIndex - integer indicating joint for a specific body
|
|
//
|
|
// Joint information includes:
|
|
// index, name, type, q-index, u-index,
|
|
// flags, joint damping, joint friction
|
|
//
|
|
// The format of the returned list is
|
|
// [int, str, int, int, int, int, float, float]
|
|
//
|
|
// TODO(hellojas): get joint positions for a body
|
|
static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) {
|
|
PyObject* pyListJointInfo;
|
|
|
|
struct b3JointInfo info;
|
|
|
|
int bodyIndex = -1;
|
|
int jointIndex = -1;
|
|
int jointInfoSize = 8; // size of struct b3JointInfo
|
|
|
|
int size = PySequence_Size(args);
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (size == 2) // get body index and joint index
|
|
{
|
|
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) {
|
|
// printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex);
|
|
|
|
pyListJointInfo = PyTuple_New(jointInfoSize);
|
|
|
|
if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) {
|
|
// printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
|
|
// info.m_jointIndex,
|
|
// info.m_jointName,
|
|
// info.m_jointType,
|
|
// info.m_qIndex,
|
|
// info.m_uIndex);
|
|
// printf(" flags=%d jointDamping=%f jointFriction=%f\n",
|
|
// info.m_flags,
|
|
// info.m_jointDamping,
|
|
// info.m_jointFriction);
|
|
PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex));
|
|
PyTuple_SetItem(pyListJointInfo, 1,
|
|
PyString_FromString(info.m_jointName));
|
|
PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType));
|
|
PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex));
|
|
PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex));
|
|
PyTuple_SetItem(pyListJointInfo, 5, PyInt_FromLong(info.m_flags));
|
|
PyTuple_SetItem(pyListJointInfo, 6,
|
|
PyFloat_FromDouble(info.m_jointDamping));
|
|
PyTuple_SetItem(pyListJointInfo, 7,
|
|
PyFloat_FromDouble(info.m_jointFriction));
|
|
return pyListJointInfo;
|
|
} else {
|
|
PyErr_SetString(SpamError, "GetJointInfo failed.");
|
|
return NULL;
|
|
}
|
|
}
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
// Returns the state of a specific joint in a given bodyIndex
|
|
//
|
|
// Args:
|
|
// bodyIndex - integer indicating body in simulation
|
|
// jointIndex - integer indicating joint for a specific body
|
|
//
|
|
// The state of a joint includes the following:
|
|
// position, velocity, force torque (6 values), and motor torque
|
|
// The returned pylist is an array of [float, float, float[6], float]
|
|
|
|
// TODO(hellojas): check accuracy of position and velocity
|
|
// TODO(hellojas): check force torque values
|
|
|
|
static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
|
|
PyObject* pyListJointForceTorque;
|
|
PyObject* pyListJointState;
|
|
|
|
|
|
struct b3JointSensorState sensorState;
|
|
|
|
int bodyIndex = -1;
|
|
int jointIndex = -1;
|
|
int sensorStateSize = 4; // size of struct b3JointSensorState
|
|
int forceTorqueSize = 6; // size of force torque list from b3JointSensorState
|
|
int j;
|
|
|
|
int size = PySequence_Size(args);
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (size == 2) // get body index and joint index
|
|
{
|
|
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) {
|
|
int status_type = 0;
|
|
b3SharedMemoryCommandHandle cmd_handle;
|
|
b3SharedMemoryStatusHandle status_handle;
|
|
|
|
|
|
if (bodyIndex < 0) {
|
|
PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex");
|
|
return NULL;
|
|
}
|
|
if (jointIndex < 0) {
|
|
PyErr_SetString(SpamError, "getJointState failed; invalid jointIndex");
|
|
return NULL;
|
|
}
|
|
|
|
|
|
cmd_handle =
|
|
b3RequestActualStateCommandInit(sm, bodyIndex);
|
|
status_handle =
|
|
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
|
|
|
|
status_type = b3GetStatusType(status_handle);
|
|
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
|
|
PyErr_SetString(SpamError, "getJointState failed.");
|
|
return NULL;
|
|
}
|
|
|
|
pyListJointState = PyTuple_New(sensorStateSize);
|
|
pyListJointForceTorque = PyTuple_New(forceTorqueSize);
|
|
|
|
b3GetJointState(sm, status_handle, jointIndex, &sensorState);
|
|
|
|
PyTuple_SetItem(pyListJointState, 0,
|
|
PyFloat_FromDouble(sensorState.m_jointPosition));
|
|
PyTuple_SetItem(pyListJointState, 1,
|
|
PyFloat_FromDouble(sensorState.m_jointVelocity));
|
|
|
|
for (j = 0; j < forceTorqueSize; j++) {
|
|
PyTuple_SetItem(pyListJointForceTorque, j,
|
|
PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
|
|
}
|
|
|
|
PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
|
|
|
|
PyTuple_SetItem(pyListJointState, 3,
|
|
PyFloat_FromDouble(sensorState.m_jointMotorTorque));
|
|
|
|
return pyListJointState;
|
|
}
|
|
} else {
|
|
PyErr_SetString(
|
|
SpamError,
|
|
"getJointState expects 2 arguments (objectUniqueId and joint index).");
|
|
return NULL;
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) {
|
|
PyObject* pyLinkState;
|
|
PyObject* pyLinkStateWorldPosition;
|
|
PyObject* pyLinkStateWorldOrientation;
|
|
PyObject* pyLinkStateLocalInertialPosition;
|
|
PyObject* pyLinkStateLocalInertialOrientation;
|
|
|
|
struct b3LinkState linkState;
|
|
|
|
int bodyIndex = -1;
|
|
int linkIndex = -1;
|
|
int i;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (PySequence_Size(args) == 2) // body index and link index
|
|
{
|
|
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &linkIndex)) {
|
|
int status_type = 0;
|
|
b3SharedMemoryCommandHandle cmd_handle;
|
|
b3SharedMemoryStatusHandle status_handle;
|
|
|
|
if (bodyIndex < 0) {
|
|
PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex");
|
|
return NULL;
|
|
}
|
|
if (linkIndex < 0) {
|
|
PyErr_SetString(SpamError, "getLinkState failed; invalid jointIndex");
|
|
return NULL;
|
|
}
|
|
|
|
|
|
cmd_handle =
|
|
b3RequestActualStateCommandInit(sm, bodyIndex);
|
|
status_handle =
|
|
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
|
|
|
|
status_type = b3GetStatusType(status_handle);
|
|
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
|
|
PyErr_SetString(SpamError, "getLinkState failed.");
|
|
return NULL;
|
|
}
|
|
|
|
b3GetLinkState(sm, status_handle, linkIndex, &linkState);
|
|
|
|
pyLinkStateWorldPosition = PyTuple_New(3);
|
|
for (i = 0; i < 3; ++i) {
|
|
PyTuple_SetItem(pyLinkStateWorldPosition, i,
|
|
PyFloat_FromDouble(linkState.m_worldPosition[i]));
|
|
}
|
|
|
|
pyLinkStateWorldOrientation = PyTuple_New(4);
|
|
for (i = 0; i < 4; ++i) {
|
|
PyTuple_SetItem(pyLinkStateWorldOrientation, i,
|
|
PyFloat_FromDouble(linkState.m_worldOrientation[i]));
|
|
}
|
|
|
|
pyLinkStateLocalInertialPosition = PyTuple_New(3);
|
|
for (i = 0; i < 3; ++i) {
|
|
PyTuple_SetItem(pyLinkStateLocalInertialPosition, i,
|
|
PyFloat_FromDouble(linkState.m_localInertialPosition[i]));
|
|
}
|
|
|
|
pyLinkStateLocalInertialOrientation = PyTuple_New(4);
|
|
for (i = 0; i < 4; ++i) {
|
|
PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i,
|
|
PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
|
|
}
|
|
|
|
pyLinkState = PyTuple_New(4);
|
|
PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
|
|
PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
|
|
PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
|
|
PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
|
|
|
|
return pyLinkState;
|
|
}
|
|
} else {
|
|
PyErr_SetString(
|
|
SpamError,
|
|
"getLinkState expects 2 arguments (objectUniqueId and link index).");
|
|
return NULL;
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
int res = 0;
|
|
|
|
PyObject* pyResultList = 0;
|
|
char* text;
|
|
double posXYZ[3];
|
|
double colorRGB[3]={1,1,1};
|
|
|
|
|
|
PyObject* textPositionObj=0;
|
|
PyObject* textColorRGBObj=0;
|
|
double textSize = 1.f;
|
|
double lifeTime = 0.f;
|
|
|
|
static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime", NULL };
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Odd", kwlist, &text, &textPositionObj, &textColorRGBObj,&textSize, &lifeTime))
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
res = pybullet_internalSetVectord(textPositionObj,posXYZ);
|
|
if (!res)
|
|
{
|
|
PyErr_SetString(SpamError, "Error converting textPositionObj[3]");
|
|
return NULL;
|
|
}
|
|
|
|
if (textColorRGBObj)
|
|
{
|
|
res = pybullet_internalSetVectord(textColorRGBObj,colorRGB);
|
|
if (!res)
|
|
{
|
|
PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]");
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
|
|
commandHandle = b3InitUserDebugDrawAddText3D(sm,text,posXYZ,colorRGB,textSize,lifeTime);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
|
|
{
|
|
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
|
|
PyObject* item = PyInt_FromLong(debugItemUniqueId);
|
|
return item;
|
|
}
|
|
|
|
PyErr_SetString(SpamError, "Error in addUserDebugText.");
|
|
return NULL;
|
|
}
|
|
|
|
|
|
static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
int res = 0;
|
|
|
|
PyObject* pyResultList = 0;
|
|
|
|
double fromXYZ[3];
|
|
double toXYZ[3];
|
|
double colorRGB[3]={1,1,1};
|
|
|
|
PyObject* lineFromObj=0;
|
|
PyObject* lineToObj=0;
|
|
PyObject* lineColorRGBObj=0;
|
|
double lineWidth = 1.f;
|
|
double lifeTime = 0.f;
|
|
|
|
static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", NULL };
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Odd", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj,&lineWidth, &lifeTime))
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
res = pybullet_internalSetVectord(lineFromObj,fromXYZ);
|
|
if (!res)
|
|
{
|
|
PyErr_SetString(SpamError, "Error converting lineFrom[3]");
|
|
return NULL;
|
|
}
|
|
|
|
res = pybullet_internalSetVectord(lineToObj,toXYZ);
|
|
if (!res)
|
|
{
|
|
PyErr_SetString(SpamError, "Error converting lineTo[3]");
|
|
return NULL;
|
|
}
|
|
if (lineColorRGBObj)
|
|
{
|
|
res = pybullet_internalSetVectord(lineColorRGBObj,colorRGB);
|
|
}
|
|
|
|
commandHandle = b3InitUserDebugDrawAddLine3D(sm,fromXYZ,toXYZ,colorRGB,lineWidth,lifeTime);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
|
|
{
|
|
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
|
|
PyObject* item = PyInt_FromLong(debugItemUniqueId);
|
|
return item;
|
|
}
|
|
|
|
PyErr_SetString(SpamError, "Error in addUserDebugLine.");
|
|
return NULL;
|
|
}
|
|
|
|
|
|
|
|
|
|
static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
int itemUniqueId;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTuple(args, "i", &itemUniqueId)) {
|
|
PyErr_SetString(SpamError, "Error parsing user debug item unique id");
|
|
return NULL;
|
|
}
|
|
|
|
commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
commandHandle = b3InitUserDebugDrawRemoveAll(sm);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
PyObject* objectColorRGBObj = 0;
|
|
double objectColorRGB[3];
|
|
|
|
int objectUniqueId = -1;
|
|
int linkIndex = -2;
|
|
|
|
static char *kwlist[] = { "objectUniqueId", "linkIndex","objectDebugColorRGB", NULL };
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|O", kwlist,
|
|
&objectUniqueId, &linkIndex, &objectColorRGBObj))
|
|
return NULL;
|
|
|
|
if (objectColorRGBObj)
|
|
{
|
|
if (pybullet_internalSetVectord(objectColorRGBObj, objectColorRGB))
|
|
{
|
|
b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm);
|
|
b3SetDebugObjectColor(commandHandle, objectUniqueId, linkIndex, objectColorRGB);
|
|
b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm);
|
|
b3RemoveDebugObjectColor(commandHandle, objectUniqueId, linkIndex);
|
|
b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
int objectUniqueId = -1;
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
struct b3VisualShapeInformation visualShapeInfo;
|
|
int statusType;
|
|
int i;
|
|
PyObject* pyResultList = 0;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
if (size == 1)
|
|
{
|
|
if (!PyArg_ParseTuple(args, "i", &objectUniqueId)) {
|
|
PyErr_SetString(SpamError, "Error parsing object unique id");
|
|
return NULL;
|
|
}
|
|
|
|
commandHandle = b3InitRequestVisualShapeInformation(sm, objectUniqueId);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
|
|
{
|
|
b3GetVisualShapeInformation(sm, &visualShapeInfo);
|
|
pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes);
|
|
for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++)
|
|
{
|
|
PyObject* visualShapeObList = PyTuple_New(7);
|
|
PyObject* item;
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId);
|
|
PyTuple_SetItem(visualShapeObList, 0, item);
|
|
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_linkIndex);
|
|
PyTuple_SetItem(visualShapeObList, 1, item);
|
|
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_visualGeometryType);
|
|
PyTuple_SetItem(visualShapeObList, 2, item);
|
|
|
|
{
|
|
PyObject* vec = PyTuple_New(3);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]);
|
|
PyTuple_SetItem(vec, 0, item);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]);
|
|
PyTuple_SetItem(vec, 1, item);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]);
|
|
PyTuple_SetItem(vec, 2, item);
|
|
PyTuple_SetItem(visualShapeObList, 3, vec);
|
|
}
|
|
|
|
item = PyString_FromString(visualShapeInfo.m_visualShapeData[i].m_meshAssetFileName);
|
|
PyTuple_SetItem(visualShapeObList, 4, item);
|
|
|
|
{
|
|
PyObject* vec = PyTuple_New(3);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[0]);
|
|
PyTuple_SetItem(vec, 0, item);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[1]);
|
|
PyTuple_SetItem(vec, 1, item);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[2]);
|
|
PyTuple_SetItem(vec, 2, item);
|
|
PyTuple_SetItem(visualShapeObList, 5, vec);
|
|
}
|
|
|
|
{
|
|
PyObject* vec = PyTuple_New(4);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[3]);
|
|
PyTuple_SetItem(vec, 0, item);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[4]);
|
|
PyTuple_SetItem(vec, 1, item);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[5]);
|
|
PyTuple_SetItem(vec, 2, item);
|
|
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[6]);
|
|
PyTuple_SetItem(vec, 3, item);
|
|
PyTuple_SetItem(visualShapeObList, 6, vec);
|
|
}
|
|
|
|
visualShapeInfo.m_visualShapeData[0].m_rgbaColor[0];
|
|
|
|
PyTuple_SetItem(pyResultList, i, visualShapeObList);
|
|
}
|
|
return pyResultList;
|
|
}
|
|
else
|
|
{
|
|
PyErr_SetString(SpamError, "Error receiving visual shape info");
|
|
return NULL;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
PyErr_SetString(SpamError, "getVisualShapeData requires 1 argument (object unique id)");
|
|
return NULL;
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
int objectUniqueId = -1;
|
|
int jointIndex = -1;
|
|
int shapeIndex = -1;
|
|
int textureUniqueId = -1;
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (size == 4)
|
|
{
|
|
if (!PyArg_ParseTuple(args, "iiii", &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId)) {
|
|
PyErr_SetString(SpamError, "Error parsing object unique id, or joint index, or shape index, or texture unique id");
|
|
return NULL;
|
|
}
|
|
|
|
commandHandle = b3InitUpdateVisualShape(sm, objectUniqueId, jointIndex, shapeIndex, textureUniqueId);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED)
|
|
{
|
|
}
|
|
else
|
|
{
|
|
PyErr_SetString(SpamError, "Error resetting visual shape info");
|
|
return NULL;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
PyErr_SetString(SpamError, "setVisualShapeData requires 4 argument");
|
|
return NULL;
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
const char* filename = 0;
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (size == 1)
|
|
{
|
|
if (!PyArg_ParseTuple(args, "s", &filename)) {
|
|
PyErr_SetString(SpamError, "Error parsing file name");
|
|
return NULL;
|
|
}
|
|
|
|
commandHandle = b3InitLoadTexture(sm, filename);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType == CMD_LOAD_TEXTURE_COMPLETED)
|
|
{
|
|
}
|
|
else
|
|
{
|
|
PyErr_SetString(SpamError, "Error loading texture");
|
|
return NULL;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
PyErr_SetString(SpamError, "loadTexture requires 1 argument");
|
|
return NULL;
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
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;
|
|
*/
|
|
|
|
int i;
|
|
|
|
PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints);
|
|
for (i = 0; i < contactPointPtr->m_numContactPoints; i++) {
|
|
PyObject* contactObList = PyTuple_New(10); // see above 10 fields
|
|
PyObject* item;
|
|
item =
|
|
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags);
|
|
PyTuple_SetItem(contactObList, 0, item);
|
|
item = PyInt_FromLong(
|
|
contactPointPtr->m_contactPointData[i].m_bodyUniqueIdA);
|
|
PyTuple_SetItem(contactObList, 1, item);
|
|
item = PyInt_FromLong(
|
|
contactPointPtr->m_contactPointData[i].m_bodyUniqueIdB);
|
|
PyTuple_SetItem(contactObList, 2, item);
|
|
item =
|
|
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexA);
|
|
PyTuple_SetItem(contactObList, 3, item);
|
|
item =
|
|
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB);
|
|
PyTuple_SetItem(contactObList, 4, item);
|
|
|
|
{
|
|
PyObject* posAObj = PyTuple_New(3);
|
|
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
|
|
PyTuple_SetItem(posAObj, 0, item);
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
|
|
PyTuple_SetItem(posAObj, 1, item);
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
|
|
PyTuple_SetItem(posAObj, 2, item);
|
|
|
|
PyTuple_SetItem(contactObList, 5, posAObj);
|
|
}
|
|
|
|
{
|
|
PyObject* posBObj = PyTuple_New(3);
|
|
|
|
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]);
|
|
PyTuple_SetItem(posBObj, 0, item);
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]);
|
|
PyTuple_SetItem(posBObj, 1, item);
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]);
|
|
PyTuple_SetItem(posBObj, 2, item);
|
|
|
|
PyTuple_SetItem(contactObList, 6, posBObj);
|
|
|
|
}
|
|
|
|
{
|
|
PyObject* normalOnB = PyTuple_New(3);
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]);
|
|
PyTuple_SetItem(normalOnB, 0, item);
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]);
|
|
PyTuple_SetItem(normalOnB, 1, item);
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]);
|
|
PyTuple_SetItem(normalOnB, 2, item);
|
|
PyTuple_SetItem(contactObList, 7, normalOnB);
|
|
}
|
|
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_contactDistance);
|
|
PyTuple_SetItem(contactObList, 8, item);
|
|
item = PyFloat_FromDouble(
|
|
contactPointPtr->m_contactPointData[i].m_normalForce);
|
|
PyTuple_SetItem(contactObList, 9, item);
|
|
|
|
PyTuple_SetItem(pyResultList, i, contactObList);
|
|
}
|
|
return pyResultList;
|
|
}
|
|
|
|
static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
PyObject* aabbMinOb=0, *aabbMaxOb=0;
|
|
double aabbMin[3];
|
|
double aabbMax[3];
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
struct b3AABBOverlapData overlapData;
|
|
int i;
|
|
|
|
static char *kwlist[] = { "aabbMin", "aabbMax", NULL };
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO", kwlist,
|
|
&aabbMinOb, &aabbMaxOb))
|
|
return NULL;
|
|
|
|
pybullet_internalSetVectord(aabbMinOb, aabbMin);
|
|
pybullet_internalSetVectord(aabbMaxOb, aabbMax);
|
|
|
|
commandHandle = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
b3GetAABBOverlapResults(sm, &overlapData);
|
|
|
|
if (overlapData.m_numOverlappingObjects)
|
|
{
|
|
PyObject* pyResultList = PyTuple_New(overlapData.m_numOverlappingObjects);
|
|
//For huge amount of overlap, we could use numpy instead (see camera pixel data)
|
|
//What would Python do with huge amount of data? Pass it onto TensorFlow!
|
|
|
|
for (i = 0; i < overlapData.m_numOverlappingObjects; i++) {
|
|
PyObject* overlap = PyTuple_New(2);//body unique id and link index
|
|
|
|
PyObject* item;
|
|
item =
|
|
PyInt_FromLong(overlapData.m_overlappingObjects[i].m_objectUniqueId);
|
|
PyTuple_SetItem(overlap, 0, item);
|
|
item =
|
|
PyInt_FromLong(overlapData.m_overlappingObjects[i].m_linkIndex);
|
|
PyTuple_SetItem(overlap, 1, item);
|
|
PyTuple_SetItem(pyResultList, i, overlap);
|
|
}
|
|
|
|
return pyResultList;
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
int bodyUniqueIdA = -1;
|
|
int bodyUniqueIdB = -1;
|
|
int linkIndexA = -2;
|
|
int linkIndexB = -2;
|
|
|
|
double distanceThreshold = 0.f;
|
|
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
struct b3ContactInformation contactPointData;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
PyObject* pyResultList = 0;
|
|
|
|
|
|
static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB",NULL };
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|ii", kwlist,
|
|
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB))
|
|
return NULL;
|
|
|
|
|
|
commandHandle = b3InitClosestDistanceQuery(sm);
|
|
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
|
|
b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
|
|
b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
|
|
if (linkIndexA >= -1)
|
|
{
|
|
b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA);
|
|
}
|
|
if (linkIndexB >= -1)
|
|
{
|
|
b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB);
|
|
}
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
|
|
|
|
|
|
b3GetContactPointInformation(sm, &contactPointData);
|
|
|
|
return MyConvertContactPoint(&contactPointData);
|
|
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
static char *kwlist[] = { "userConstraintUniqueId",NULL};
|
|
int userConstraintUniqueId=-1;
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
|
|
|
|
if (0 == sm)
|
|
{
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", kwlist,&userConstraintUniqueId))
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
};
|
|
|
|
|
|
/*
|
|
static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
return NULL;
|
|
}
|
|
*/
|
|
|
|
static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
int size = PySequence_Size(args);
|
|
int bodyUniqueIdA = -1;
|
|
int bodyUniqueIdB = -1;
|
|
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
int parentBodyUniqueId=-1;
|
|
int parentLinkIndex=-1;
|
|
int childBodyUniqueId=-1;
|
|
int childLinkIndex=-1;
|
|
int jointType=ePoint2PointType;
|
|
PyObject* jointAxisObj=0;
|
|
double jointAxis[3]={0,0,0};
|
|
PyObject* parentFramePositionObj = 0;
|
|
double parentFramePosition[3]={0,0,0};
|
|
PyObject* childFramePositionObj = 0;
|
|
double childFramePosition[3]={0,0,0};
|
|
PyObject* parentFrameOrientationObj = 0;
|
|
double parentFrameOrientation[4]={0,0,0,1};
|
|
PyObject* childFrameOrientationObj = 0;
|
|
double childFrameOrientation[4]={0,0,0,1};
|
|
|
|
struct b3JointInfo jointInfo;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
PyObject* pyResultList = 0;
|
|
|
|
|
|
static char *kwlist[] = { "parentBodyUniqueId", "parentLinkIndex",
|
|
"childBodyUniqueId", "childLinkIndex",
|
|
"jointType",
|
|
"jointAxis",
|
|
"parentFramePosition",
|
|
"childFramePosition",
|
|
"parentFrameOrientation",
|
|
"childFrameOrientation",
|
|
NULL };
|
|
|
|
if (0 == sm)
|
|
{
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OO", kwlist,&parentBodyUniqueId,&parentLinkIndex,
|
|
&childBodyUniqueId,&childLinkIndex,
|
|
&jointType,&jointAxisObj,
|
|
&parentFramePositionObj,
|
|
&childFramePositionObj,
|
|
&parentFrameOrientationObj,
|
|
&childFrameOrientationObj
|
|
))
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
pybullet_internalSetVectord(jointAxisObj,jointAxis);
|
|
pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition);
|
|
pybullet_internalSetVectord(childFramePositionObj,childFramePosition);
|
|
pybullet_internalSetVector4d(parentFrameOrientationObj,parentFrameOrientation);
|
|
pybullet_internalSetVector4d(childFrameOrientationObj,childFrameOrientation);
|
|
|
|
jointInfo.m_jointType = jointType;
|
|
jointInfo.m_parentFrame[0] = parentFramePosition[0];
|
|
jointInfo.m_parentFrame[1] = parentFramePosition[1];
|
|
jointInfo.m_parentFrame[2] = parentFramePosition[2];
|
|
jointInfo.m_parentFrame[3] = parentFrameOrientation[0];
|
|
jointInfo.m_parentFrame[4] = parentFrameOrientation[1];
|
|
jointInfo.m_parentFrame[5] = parentFrameOrientation[2];
|
|
jointInfo.m_parentFrame[6] = parentFrameOrientation[3];
|
|
|
|
jointInfo.m_childFrame[0] = childFramePosition[0];
|
|
jointInfo.m_childFrame[1] = childFramePosition[1];
|
|
jointInfo.m_childFrame[2] = childFramePosition[2];
|
|
jointInfo.m_childFrame[3] = childFrameOrientation[0];
|
|
jointInfo.m_childFrame[4] = childFrameOrientation[1];
|
|
jointInfo.m_childFrame[5] = childFrameOrientation[2];
|
|
jointInfo.m_childFrame[6] = childFrameOrientation[3];
|
|
|
|
jointInfo.m_jointAxis[0] = jointAxis[0];
|
|
jointInfo.m_jointAxis[1] = jointAxis[1];
|
|
jointInfo.m_jointAxis[2] = jointAxis[2];
|
|
|
|
commandHandle = b3InitCreateUserConstraintCommand(sm, parentBodyUniqueId, parentLinkIndex, childBodyUniqueId, childLinkIndex, &jointInfo);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType==CMD_USER_CONSTRAINT_COMPLETED)
|
|
{
|
|
int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
|
|
PyObject* ob = PyLong_FromLong(userConstraintUid);
|
|
return ob;
|
|
} else
|
|
{
|
|
PyErr_SetString(SpamError, "createConstraint failed.");
|
|
return NULL;
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
|
|
}
|
|
|
|
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) {
|
|
int size = PySequence_Size(args);
|
|
int bodyUniqueIdA = -1;
|
|
int bodyUniqueIdB = -1;
|
|
|
|
b3SharedMemoryCommandHandle commandHandle;
|
|
struct b3ContactInformation contactPointData;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
PyObject* pyResultList = 0;
|
|
|
|
|
|
static char *kwlist[] = { "bodyA", "bodyB", NULL };
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist,
|
|
&bodyUniqueIdA, &bodyUniqueIdB))
|
|
return NULL;
|
|
|
|
|
|
commandHandle = b3InitRequestContactPointInformation(sm);
|
|
b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
|
|
b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
|
|
//b3SetContactQueryMode(commandHandle, mode);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
|
|
|
|
b3GetContactPointInformation(sm, &contactPointData);
|
|
|
|
return MyConvertContactPoint(&contactPointData);
|
|
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
|
|
/// Render an image from the current timestep of the simulation, width, height are required, other args are optional
|
|
// getCameraImage(w, h, view[16], projection[16], lightDir[3], lightColor[3], lightDist, hasShadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff)
|
|
static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
/// request an image from a simulated camera, using a software renderer.
|
|
struct b3CameraImageData imageData;
|
|
PyObject* objViewMat = 0, *objProjMat = 0, *lightDirObj = 0, *lightColorObj = 0;
|
|
int width, height;
|
|
int size = PySequence_Size(args);
|
|
float viewMatrix[16];
|
|
float projectionMatrix[16];
|
|
float lightDir[3];
|
|
float lightColor[3];
|
|
float lightDist = 10.0;
|
|
int hasShadow = 0;
|
|
float lightAmbientCoeff = 0.6;
|
|
float lightDiffuseCoeff = 0.35;
|
|
float lightSpecularCoeff = 0.05;
|
|
// inialize cmd
|
|
b3SharedMemoryCommandHandle command;
|
|
|
|
if (0 == sm)
|
|
{
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
command = b3InitRequestCameraImage(sm);
|
|
|
|
// set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow
|
|
static char *kwlist[] = { "width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", NULL };
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifff", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff))
|
|
{
|
|
return NULL;
|
|
}
|
|
b3RequestCameraImageSetPixelResolution(command, width, height);
|
|
|
|
// set camera matrices only if set matrix function succeeds
|
|
if (pybullet_internalSetMatrix(objViewMat, viewMatrix) && (pybullet_internalSetMatrix(objProjMat, projectionMatrix)))
|
|
{
|
|
b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix);
|
|
}
|
|
//set light direction only if function succeeds
|
|
if (pybullet_internalSetVector(lightDirObj, lightDir))
|
|
{
|
|
b3RequestCameraImageSetLightDirection(command, lightDir);
|
|
}
|
|
//set light color only if function succeeds
|
|
if (pybullet_internalSetVector(lightColorObj, lightColor))
|
|
{
|
|
b3RequestCameraImageSetLightColor(command, lightColor);
|
|
}
|
|
|
|
b3RequestCameraImageSetLightDistance(command, lightDist);
|
|
|
|
b3RequestCameraImageSetShadow(command, hasShadow);
|
|
|
|
b3RequestCameraImageSetLightAmbientCoeff(command, lightAmbientCoeff);
|
|
b3RequestCameraImageSetLightDiffuseCoeff(command, lightDiffuseCoeff);
|
|
b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff);
|
|
|
|
if (b3CanSubmitCommand(sm))
|
|
{
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
|
|
// b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType == CMD_CAMERA_IMAGE_COMPLETED) {
|
|
PyObject* item2;
|
|
PyObject* pyResultList; // store 4 elements in this result: width,
|
|
// height, rgbData, depth
|
|
|
|
#ifdef PYBULLET_USE_NUMPY
|
|
PyObject* pyRGB;
|
|
PyObject* pyDep;
|
|
PyObject* pySeg;
|
|
|
|
int i, j, p;
|
|
|
|
b3GetCameraImageData(sm, &imageData);
|
|
// TODO(hellojas): error handling if image size is 0
|
|
pyResultList = PyTuple_New(5);
|
|
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
|
|
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
|
|
|
|
int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values
|
|
|
|
npy_intp rgb_dims[3] = { imageData.m_pixelHeight, imageData.m_pixelWidth,
|
|
bytesPerPixel };
|
|
npy_intp dep_dims[2] = { imageData.m_pixelHeight, imageData.m_pixelWidth };
|
|
npy_intp seg_dims[2] = { imageData.m_pixelHeight, imageData.m_pixelWidth };
|
|
|
|
pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8);
|
|
pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32);
|
|
pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32);
|
|
|
|
memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData,
|
|
imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel);
|
|
memcpy(PyArray_DATA(pyDep), imageData.m_depthValues,
|
|
imageData.m_pixelHeight * imageData.m_pixelWidth);
|
|
memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues,
|
|
imageData.m_pixelHeight * imageData.m_pixelWidth);
|
|
|
|
PyTuple_SetItem(pyResultList, 2, pyRGB);
|
|
PyTuple_SetItem(pyResultList, 3, pyDep);
|
|
PyTuple_SetItem(pyResultList, 4, pySeg);
|
|
#else//PYBULLET_USE_NUMPY
|
|
PyObject* pylistRGB;
|
|
PyObject* pylistDep;
|
|
PyObject* pylistSeg;
|
|
|
|
int i, j, p;
|
|
|
|
b3GetCameraImageData(sm, &imageData);
|
|
// TODO(hellojas): error handling if image size is 0
|
|
pyResultList = PyTuple_New(5);
|
|
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
|
|
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
|
|
|
|
{
|
|
PyObject* item;
|
|
int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values
|
|
int num =
|
|
bytesPerPixel * imageData.m_pixelWidth * imageData.m_pixelHeight;
|
|
pylistRGB = PyTuple_New(num);
|
|
pylistDep =
|
|
PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight);
|
|
pylistSeg =
|
|
PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight);
|
|
for (i = 0; i < imageData.m_pixelWidth; i++) {
|
|
for (j = 0; j < imageData.m_pixelHeight; j++) {
|
|
// TODO(hellojas): validate depth values make sense
|
|
int depIndex = i + j * imageData.m_pixelWidth;
|
|
{
|
|
item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
|
|
PyTuple_SetItem(pylistDep, depIndex, item);
|
|
}
|
|
{
|
|
item2 =
|
|
PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]);
|
|
PyTuple_SetItem(pylistSeg, depIndex, item2);
|
|
}
|
|
|
|
for (p = 0; p < bytesPerPixel; p++) {
|
|
int pixelIndex =
|
|
bytesPerPixel * (i + j * imageData.m_pixelWidth) + p;
|
|
item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]);
|
|
PyTuple_SetItem(pylistRGB, pixelIndex, item);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
PyTuple_SetItem(pyResultList, 2, pylistRGB);
|
|
PyTuple_SetItem(pyResultList, 3, pylistDep);
|
|
PyTuple_SetItem(pyResultList, 4, pylistSeg);
|
|
return pyResultList;
|
|
#endif//PYBULLET_USE_NUMPY
|
|
|
|
return pyResultList;
|
|
}
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
|
|
}
|
|
|
|
|
|
|
|
static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
PyObject* camEyeObj = 0;
|
|
PyObject* camTargetPositionObj = 0;
|
|
PyObject* camUpVectorObj = 0;
|
|
float camEye[3];
|
|
float camTargetPosition[3];
|
|
float camUpVector[3];
|
|
|
|
// set camera resolution, optionally view, projection matrix, light position
|
|
static char *kwlist[] = { "cameraEyePosition", "cameraTargetPosition", "cameraUpVector",NULL };
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOO", kwlist, &camEyeObj, &camTargetPositionObj, &camUpVectorObj))
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
if (pybullet_internalSetVector(camEyeObj, camEye) &&
|
|
pybullet_internalSetVector(camTargetPositionObj, camTargetPosition) &&
|
|
pybullet_internalSetVector(camUpVectorObj, camUpVector))
|
|
{
|
|
float viewMatrix[16];
|
|
PyObject* pyResultList=0;
|
|
int i;
|
|
b3ComputeViewMatrixFromPositions(camEye, camTargetPosition, camUpVector, viewMatrix);
|
|
|
|
pyResultList = PyTuple_New(16);
|
|
for (i = 0; i < 16; i++)
|
|
{
|
|
PyObject* item = PyFloat_FromDouble(viewMatrix[i]);
|
|
PyTuple_SetItem(pyResultList, i, item);
|
|
}
|
|
return pyResultList;
|
|
}
|
|
|
|
PyErr_SetString(SpamError, "Error in computeViewMatrix.");
|
|
return NULL;
|
|
}
|
|
|
|
///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices
|
|
static PyObject* pybullet_computeViewMatrixFromYawPitchRoll(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
PyObject* cameraTargetPositionObj = 0;
|
|
float cameraTargetPosition[3];
|
|
float distance, yaw, pitch, roll;
|
|
int upAxisIndex;
|
|
float viewMatrix[16];
|
|
PyObject* pyResultList = 0;
|
|
int i;
|
|
|
|
// set camera resolution, optionally view, projection matrix, light position
|
|
static char *kwlist[] = { "cameraTargetPosition", "distance", "yaw", "pitch", "roll", "upAxisIndex" ,NULL };
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "Offffi", kwlist, &cameraTargetPositionObj, &distance,&yaw,&pitch,&roll, &upAxisIndex))
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
if (!pybullet_internalSetVector(cameraTargetPositionObj, cameraTargetPosition))
|
|
{
|
|
PyErr_SetString(SpamError, "Cannot convert cameraTargetPosition.");
|
|
return NULL;
|
|
}
|
|
|
|
b3ComputeViewMatrixFromYawPitchRoll(cameraTargetPosition, distance, yaw, pitch, roll, upAxisIndex, viewMatrix);
|
|
|
|
pyResultList = PyTuple_New(16);
|
|
for (i = 0; i < 16; i++)
|
|
{
|
|
PyObject* item = PyFloat_FromDouble(viewMatrix[i]);
|
|
PyTuple_SetItem(pyResultList, i, item);
|
|
}
|
|
return pyResultList;
|
|
|
|
|
|
}
|
|
|
|
///compute a projection matrix, helper function for b3RequestCameraImageSetCameraMatrices
|
|
static PyObject* pybullet_computeProjectionMatrix(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
PyObject* pyResultList = 0;
|
|
float left;
|
|
float right;
|
|
float bottom;
|
|
float top;
|
|
float nearVal;
|
|
float farVal;
|
|
float projectionMatrix[16];
|
|
int i;
|
|
|
|
// set camera resolution, optionally view, projection matrix, light position
|
|
static char *kwlist[] = { "left", "right", "bottom", "top", "nearVal", "farVal" ,NULL };
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ffffff", kwlist, &left, &right, &bottom, &top, &nearVal, &farVal))
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
|
|
b3ComputeProjectionMatrix(left, right, bottom, top, nearVal, farVal, projectionMatrix);
|
|
|
|
pyResultList = PyTuple_New(16);
|
|
for (i = 0; i < 16; i++)
|
|
{
|
|
PyObject* item = PyFloat_FromDouble(projectionMatrix[i]);
|
|
PyTuple_SetItem(pyResultList, i, item);
|
|
}
|
|
return pyResultList;
|
|
|
|
}
|
|
|
|
static PyObject* pybullet_computeProjectionMatrixFOV(PyObject* self, PyObject* args, PyObject *keywds)
|
|
{
|
|
float fov, aspect, nearVal, farVal;
|
|
PyObject* pyResultList = 0;
|
|
float projectionMatrix[16];
|
|
int i;
|
|
|
|
static char *kwlist[] = { "fov","aspect","nearVal","farVal",NULL };
|
|
|
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ffff", kwlist, &fov, &aspect, &nearVal, &farVal))
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
b3ComputeProjectionMatrixFOV(fov, aspect, nearVal, farVal, projectionMatrix);
|
|
|
|
pyResultList = PyTuple_New(16);
|
|
for (i = 0; i < 16; i++)
|
|
{
|
|
PyObject* item = PyFloat_FromDouble(projectionMatrix[i]);
|
|
PyTuple_SetItem(pyResultList, i, item);
|
|
}
|
|
return pyResultList;
|
|
|
|
}
|
|
|
|
|
|
// Render an image from the current timestep of the simulation
|
|
//
|
|
// Examples:
|
|
// renderImage() - default image resolution and camera position
|
|
// renderImage(w, h) - image resolution of (w,h), default camera
|
|
// renderImage(w, h, view[16], projection[16]) - set both resolution
|
|
// and initialize camera to the view and projection values
|
|
// renderImage(w, h, cameraPos, targetPos, cameraUp, nearVal, farVal) - set
|
|
// resolution and initialize camera based on camera position, target
|
|
// position, camera up and fulstrum near/far values.
|
|
// renderImage(w, h, cameraPos, targetPos, cameraUp, nearVal, farVal, fov) -
|
|
// set resolution and initialize camera based on camera position, target
|
|
// position, camera up, fulstrum near/far values and camera field of view.
|
|
// renderImage(w, h, targetPos, distance, yaw, pitch, upAxisIndex, nearVal,
|
|
// farVal, fov)
|
|
|
|
//
|
|
// Note if the (w,h) is too small, the objects may not appear based on
|
|
// where the camera has been set
|
|
//
|
|
// TODO(hellojas): fix image is cut off at head
|
|
// TODO(hellojas): should we add check to give minimum image resolution
|
|
// to see object based on camera position?
|
|
static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) {
|
|
/// request an image from a simulated camera, using a software renderer.
|
|
struct b3CameraImageData imageData;
|
|
PyObject* objViewMat, *objProjMat;
|
|
PyObject* objCameraPos, *objTargetPos, *objCameraUp;
|
|
|
|
int width, height;
|
|
int size = PySequence_Size(args);
|
|
float viewMatrix[16];
|
|
float projectionMatrix[16];
|
|
|
|
float cameraPos[3];
|
|
float targetPos[3];
|
|
float cameraUp[3];
|
|
|
|
float left, right, bottom, top, aspect;
|
|
float nearVal, farVal;
|
|
float fov;
|
|
|
|
// inialize cmd
|
|
b3SharedMemoryCommandHandle command;
|
|
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
command = b3InitRequestCameraImage(sm);
|
|
|
|
if (size == 2) // only set camera resolution
|
|
{
|
|
if (PyArg_ParseTuple(args, "ii", &width, &height)) {
|
|
b3RequestCameraImageSetPixelResolution(command, width, height);
|
|
}
|
|
} else if (size == 4) // set camera resolution and view and projection matrix
|
|
{
|
|
if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat,
|
|
&objProjMat)) {
|
|
b3RequestCameraImageSetPixelResolution(command, width, height);
|
|
|
|
// set camera matrices only if set matrix function succeeds
|
|
if (pybullet_internalSetMatrix(objViewMat, viewMatrix) &&
|
|
(pybullet_internalSetMatrix(objProjMat, projectionMatrix))) {
|
|
b3RequestCameraImageSetCameraMatrices(command, viewMatrix,
|
|
projectionMatrix);
|
|
} else {
|
|
PyErr_SetString(SpamError, "Error parsing view or projection matrix.");
|
|
return NULL;
|
|
}
|
|
}
|
|
} else if (size == 7) // set camera resolution, camera positions and
|
|
// calculate projection using near/far values.
|
|
{
|
|
if (PyArg_ParseTuple(args, "iiOOOff", &width, &height, &objCameraPos,
|
|
&objTargetPos, &objCameraUp, &nearVal, &farVal)) {
|
|
b3RequestCameraImageSetPixelResolution(command, width, height);
|
|
if (pybullet_internalSetVector(objCameraPos, cameraPos) &&
|
|
pybullet_internalSetVector(objTargetPos, targetPos) &&
|
|
pybullet_internalSetVector(objCameraUp, cameraUp))
|
|
{
|
|
b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos,
|
|
cameraUp);
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"Error parsing camera position, target or up.");
|
|
return NULL;
|
|
}
|
|
|
|
aspect = width / height;
|
|
left = -aspect * nearVal;
|
|
right = aspect * nearVal;
|
|
bottom = -nearVal;
|
|
top = nearVal;
|
|
b3RequestCameraImageSetProjectionMatrix(command, left, right, bottom, top,
|
|
nearVal, farVal);
|
|
}
|
|
} else if (size == 8) // set camera resolution, camera positions and
|
|
// calculate projection using near/far values & field
|
|
// of view
|
|
{
|
|
if (PyArg_ParseTuple(args, "iiOOOfff", &width, &height, &objCameraPos,
|
|
&objTargetPos, &objCameraUp, &nearVal, &farVal,
|
|
&fov)) {
|
|
b3RequestCameraImageSetPixelResolution(command, width, height);
|
|
if (pybullet_internalSetVector(objCameraPos, cameraPos) &&
|
|
pybullet_internalSetVector(objTargetPos, targetPos) &&
|
|
pybullet_internalSetVector(objCameraUp, cameraUp)) {
|
|
b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos,
|
|
cameraUp);
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"Error parsing camera position, target or up.");
|
|
return NULL;
|
|
}
|
|
|
|
aspect = width / height;
|
|
b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal,
|
|
farVal);
|
|
}
|
|
} else if (size == 11) {
|
|
int upAxisIndex = 1;
|
|
float camDistance, yaw, pitch, roll;
|
|
|
|
// sometimes more arguments are better :-)
|
|
if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos,
|
|
&camDistance, &yaw, &pitch, &roll, &upAxisIndex,
|
|
&nearVal, &farVal, &fov)) {
|
|
b3RequestCameraImageSetPixelResolution(command, width, height);
|
|
if (pybullet_internalSetVector(objTargetPos, targetPos)) {
|
|
// printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f,
|
|
// yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f,
|
|
// fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov);
|
|
|
|
b3RequestCameraImageSetViewMatrix2(command, targetPos, camDistance, yaw,
|
|
pitch, roll, upAxisIndex);
|
|
aspect = width / height;
|
|
b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect,
|
|
nearVal, farVal);
|
|
} else {
|
|
PyErr_SetString(SpamError, "Error parsing camera target pos");
|
|
}
|
|
} else {
|
|
PyErr_SetString(SpamError, "Error parsing arguments");
|
|
}
|
|
|
|
} else {
|
|
PyErr_SetString(SpamError, "Invalid number of args passed to renderImage.");
|
|
return NULL;
|
|
}
|
|
|
|
if (b3CanSubmitCommand(sm)) {
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
|
|
// b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL);
|
|
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
statusType = b3GetStatusType(statusHandle);
|
|
if (statusType == CMD_CAMERA_IMAGE_COMPLETED) {
|
|
PyObject* item2;
|
|
PyObject* pyResultList; // store 4 elements in this result: width,
|
|
// height, rgbData, depth
|
|
|
|
#ifdef PYBULLET_USE_NUMPY
|
|
PyObject* pyRGB;
|
|
PyObject* pyDep;
|
|
PyObject* pySeg;
|
|
|
|
int i, j, p;
|
|
|
|
b3GetCameraImageData(sm, &imageData);
|
|
// TODO(hellojas): error handling if image size is 0
|
|
pyResultList = PyTuple_New(5);
|
|
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
|
|
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
|
|
|
|
int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values
|
|
|
|
npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth,
|
|
bytesPerPixel};
|
|
npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth};
|
|
npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth};
|
|
|
|
pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8);
|
|
pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32);
|
|
pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32);
|
|
|
|
memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData,
|
|
imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel);
|
|
memcpy(PyArray_DATA(pyDep), imageData.m_depthValues,
|
|
imageData.m_pixelHeight * imageData.m_pixelWidth);
|
|
memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues,
|
|
imageData.m_pixelHeight * imageData.m_pixelWidth);
|
|
|
|
PyTuple_SetItem(pyResultList, 2, pyRGB);
|
|
PyTuple_SetItem(pyResultList, 3, pyDep);
|
|
PyTuple_SetItem(pyResultList, 4, pySeg);
|
|
#else//PYBULLET_USE_NUMPY
|
|
PyObject* pylistRGB;
|
|
PyObject* pylistDep;
|
|
PyObject* pylistSeg;
|
|
|
|
int i, j, p;
|
|
|
|
b3GetCameraImageData(sm, &imageData);
|
|
// TODO(hellojas): error handling if image size is 0
|
|
pyResultList = PyTuple_New(5);
|
|
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
|
|
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
|
|
|
|
{
|
|
PyObject* item;
|
|
int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values
|
|
int num =
|
|
bytesPerPixel * imageData.m_pixelWidth * imageData.m_pixelHeight;
|
|
pylistRGB = PyTuple_New(num);
|
|
pylistDep =
|
|
PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight);
|
|
pylistSeg =
|
|
PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight);
|
|
for (i = 0; i < imageData.m_pixelWidth; i++) {
|
|
for (j = 0; j < imageData.m_pixelHeight; j++) {
|
|
// TODO(hellojas): validate depth values make sense
|
|
int depIndex = i + j * imageData.m_pixelWidth;
|
|
{
|
|
item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
|
|
PyTuple_SetItem(pylistDep, depIndex, item);
|
|
}
|
|
{
|
|
item2 =
|
|
PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]);
|
|
PyTuple_SetItem(pylistSeg, depIndex, item2);
|
|
}
|
|
|
|
for (p = 0; p < bytesPerPixel; p++) {
|
|
int pixelIndex =
|
|
bytesPerPixel * (i + j * imageData.m_pixelWidth) + p;
|
|
item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]);
|
|
PyTuple_SetItem(pylistRGB, pixelIndex, item);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
PyTuple_SetItem(pyResultList, 2, pylistRGB);
|
|
PyTuple_SetItem(pyResultList, 3, pylistDep);
|
|
PyTuple_SetItem(pyResultList, 4, pylistSeg);
|
|
return pyResultList;
|
|
#endif//PYBULLET_USE_NUMPY
|
|
|
|
return pyResultList;
|
|
}
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args) {
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
{
|
|
int objectUniqueId, linkIndex, flags;
|
|
double force[3];
|
|
double position[3] = {0.0, 0.0, 0.0};
|
|
PyObject* forceObj, *posObj;
|
|
|
|
b3SharedMemoryCommandHandle command;
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int size = PySequence_Size(args);
|
|
|
|
if (size == 5) {
|
|
if (!PyArg_ParseTuple(args, "iiOOi", &objectUniqueId, &linkIndex,
|
|
&forceObj, &posObj, &flags)) {
|
|
PyErr_SetString(SpamError, "applyBaseForce couldn't parse arguments");
|
|
return NULL;
|
|
}
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"applyBaseForce needs 5 arguments: objectUniqueId, "
|
|
"linkIndex (-1 for base/root link), force [x,y,z], "
|
|
"position [x,y,z], flags");
|
|
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
PyObject* seq;
|
|
int len, i;
|
|
seq = PySequence_Fast(forceObj, "expected a sequence");
|
|
len = PySequence_Size(forceObj);
|
|
if (len == 3) {
|
|
for (i = 0; i < 3; i++) {
|
|
force[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
} else {
|
|
PyErr_SetString(SpamError, "force needs a 3 coordinates [x,y,z].");
|
|
Py_DECREF(seq);
|
|
return NULL;
|
|
}
|
|
Py_DECREF(seq);
|
|
}
|
|
{
|
|
PyObject* seq;
|
|
int len, i;
|
|
seq = PySequence_Fast(posObj, "expected a sequence");
|
|
len = PySequence_Size(posObj);
|
|
if (len == 3) {
|
|
for (i = 0; i < 3; i++) {
|
|
position[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
} else {
|
|
PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
|
|
Py_DECREF(seq);
|
|
return NULL;
|
|
}
|
|
Py_DECREF(seq);
|
|
}
|
|
if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) {
|
|
PyErr_SetString(SpamError,
|
|
"flag has to be either WORLD_FRAME or LINK_FRAME");
|
|
return NULL;
|
|
}
|
|
command = b3ApplyExternalForceCommandInit(sm);
|
|
b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position,
|
|
flags);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args) {
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
{
|
|
int objectUniqueId, linkIndex, flags;
|
|
double torque[3];
|
|
PyObject* torqueObj;
|
|
|
|
if (PyArg_ParseTuple(args, "iiOi", &objectUniqueId, &linkIndex, &torqueObj,
|
|
&flags)) {
|
|
PyObject* seq;
|
|
int len, i;
|
|
seq = PySequence_Fast(torqueObj, "expected a sequence");
|
|
len = PySequence_Size(torqueObj);
|
|
if (len == 3) {
|
|
for (i = 0; i < 3; i++) {
|
|
torque[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
} else {
|
|
PyErr_SetString(SpamError, "torque needs a 3 coordinates [x,y,z].");
|
|
Py_DECREF(seq);
|
|
return NULL;
|
|
}
|
|
Py_DECREF(seq);
|
|
|
|
if (linkIndex < -1) {
|
|
PyErr_SetString(SpamError,
|
|
"Invalid link index, has to be -1 or larger");
|
|
return NULL;
|
|
}
|
|
if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) {
|
|
PyErr_SetString(SpamError,
|
|
"flag has to be either WORLD_FRAME or LINK_FRAME");
|
|
return NULL;
|
|
}
|
|
{
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
b3SharedMemoryCommandHandle command =
|
|
b3ApplyExternalForceCommandInit(sm);
|
|
b3ApplyExternalTorque(command, objectUniqueId, -1, torque, flags);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
|
}
|
|
}
|
|
}
|
|
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyObject* pybullet_getQuaternionFromEuler(PyObject* self,
|
|
PyObject* args) {
|
|
double rpy[3];
|
|
|
|
PyObject* eulerObj;
|
|
|
|
if (PyArg_ParseTuple(args, "O", &eulerObj)) {
|
|
PyObject* seq;
|
|
int len, i;
|
|
seq = PySequence_Fast(eulerObj, "expected a sequence");
|
|
len = PySequence_Size(eulerObj);
|
|
if (len == 3) {
|
|
for (i = 0; i < 3; i++) {
|
|
rpy[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"Euler angles need a 3 coordinates [roll, pitch, yaw].");
|
|
Py_DECREF(seq);
|
|
return NULL;
|
|
}
|
|
Py_DECREF(seq);
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"Euler angles need a 3 coordinates [roll, pitch, yaw].");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
double phi, the, psi;
|
|
double roll = rpy[0];
|
|
double pitch = rpy[1];
|
|
double yaw = rpy[2];
|
|
phi = roll / 2.0;
|
|
the = pitch / 2.0;
|
|
psi = yaw / 2.0;
|
|
{
|
|
double quat[4] = {
|
|
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
|
|
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
|
|
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
|
|
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)};
|
|
|
|
// normalize the quaternion
|
|
double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] +
|
|
quat[2] * quat[2] + quat[3] * quat[3]);
|
|
quat[0] /= len;
|
|
quat[1] /= len;
|
|
quat[2] /= len;
|
|
quat[3] /= len;
|
|
{
|
|
PyObject* pylist;
|
|
int i;
|
|
pylist = PyTuple_New(4);
|
|
for (i = 0; i < 4; i++)
|
|
PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quat[i]));
|
|
return pylist;
|
|
}
|
|
}
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
/// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
|
|
/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
|
|
static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
|
|
PyObject* args) {
|
|
double squ;
|
|
double sqx;
|
|
double sqy;
|
|
double sqz;
|
|
|
|
double quat[4];
|
|
|
|
PyObject* quatObj;
|
|
|
|
if (PyArg_ParseTuple(args, "O", &quatObj)) {
|
|
PyObject* seq;
|
|
int len, i;
|
|
seq = PySequence_Fast(quatObj, "expected a sequence");
|
|
len = PySequence_Size(quatObj);
|
|
if (len == 4) {
|
|
for (i = 0; i < 4; i++) {
|
|
quat[i] = pybullet_internalGetFloatFromSequence(seq, i);
|
|
}
|
|
} else {
|
|
PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
|
|
Py_DECREF(seq);
|
|
return NULL;
|
|
}
|
|
Py_DECREF(seq);
|
|
} else {
|
|
PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
|
|
return NULL;
|
|
}
|
|
|
|
{
|
|
double rpy[3];
|
|
double sarg;
|
|
sqx = quat[0] * quat[0];
|
|
sqy = quat[1] * quat[1];
|
|
sqz = quat[2] * quat[2];
|
|
squ = quat[3] * quat[3];
|
|
rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]),
|
|
squ - sqx - sqy + sqz);
|
|
sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]);
|
|
rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538
|
|
: (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg));
|
|
rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]),
|
|
squ + sqx - sqy - sqz);
|
|
{
|
|
PyObject* pylist;
|
|
int i;
|
|
pylist = PyTuple_New(3);
|
|
for (i = 0; i < 3; i++)
|
|
PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i]));
|
|
return pylist;
|
|
}
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
///Experimental Inverse Kinematics binding ,7-dof KUKA IIWA only
|
|
static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
|
|
PyObject* args) {
|
|
int size;
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
size = PySequence_Size(args);
|
|
if (size == 2)
|
|
{
|
|
int bodyIndex;
|
|
int endEffectorLinkIndex;
|
|
|
|
PyObject* targetPosObj;
|
|
PyObject* targetOrnObj;
|
|
|
|
if (PyArg_ParseTuple(args, "iiOO", &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj))
|
|
{
|
|
double pos[3];
|
|
double ori[4]={0,1.0,0,0};
|
|
|
|
if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori))
|
|
{
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int numPos=0;
|
|
int resultBodyIndex;
|
|
int result;
|
|
|
|
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex);
|
|
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori);
|
|
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++)
|
|
{
|
|
PyTuple_SetItem(pylist, i,
|
|
PyFloat_FromDouble(ikOutPutJointPos[i]));
|
|
}
|
|
|
|
free(ikOutPutJointPos);
|
|
return pylist;
|
|
}
|
|
else
|
|
{
|
|
PyErr_SetString(SpamError,
|
|
"Error in calculateInverseKinematics");
|
|
return NULL;
|
|
}
|
|
} else
|
|
{
|
|
PyErr_SetString(SpamError,
|
|
"calculateInverseKinematics couldn't extract position vector3");
|
|
return NULL;
|
|
}
|
|
}
|
|
} else
|
|
{
|
|
PyErr_SetString(SpamError,
|
|
"calculateInverseKinematics expects 2 arguments, body index, "
|
|
"and target position for end effector");
|
|
return NULL;
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
|
|
|
|
/// 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) {
|
|
int size;
|
|
if (0 == sm) {
|
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
|
return NULL;
|
|
}
|
|
|
|
size = PySequence_Size(args);
|
|
if (size == 4) {
|
|
int bodyIndex;
|
|
PyObject* objPositionsQ;
|
|
PyObject* objVelocitiesQdot;
|
|
PyObject* objAccelerations;
|
|
|
|
if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ,
|
|
&objVelocitiesQdot, &objAccelerations)) {
|
|
int szObPos = PySequence_Size(objPositionsQ);
|
|
int szObVel = PySequence_Size(objVelocitiesQdot);
|
|
int szObAcc = PySequence_Size(objAccelerations);
|
|
int numJoints = b3GetNumJoints(sm, bodyIndex);
|
|
if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) &&
|
|
(szObAcc == numJoints)) {
|
|
int szInBytes = sizeof(double) * numJoints;
|
|
int i;
|
|
PyObject* pylist = 0;
|
|
double* jointPositionsQ = (double*)malloc(szInBytes);
|
|
double* jointVelocitiesQdot = (double*)malloc(szInBytes);
|
|
double* jointAccelerations = (double*)malloc(szInBytes);
|
|
double* jointForcesOutput = (double*)malloc(szInBytes);
|
|
|
|
for (i = 0; i < numJoints; i++) {
|
|
jointPositionsQ[i] =
|
|
pybullet_internalGetFloatFromSequence(objPositionsQ, i);
|
|
jointVelocitiesQdot[i] =
|
|
pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i);
|
|
jointAccelerations[i] =
|
|
pybullet_internalGetFloatFromSequence(objAccelerations, i);
|
|
}
|
|
|
|
{
|
|
b3SharedMemoryStatusHandle statusHandle;
|
|
int statusType;
|
|
b3SharedMemoryCommandHandle commandHandle =
|
|
b3CalculateInverseDynamicsCommandInit(
|
|
sm, bodyIndex, jointPositionsQ, jointVelocitiesQdot,
|
|
jointAccelerations);
|
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
|
|
|
statusType = b3GetStatusType(statusHandle);
|
|
|
|
if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) {
|
|
int bodyUniqueId;
|
|
int dofCount;
|
|
|
|
b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,
|
|
&dofCount, 0);
|
|
|
|
if (dofCount) {
|
|
b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,
|
|
jointForcesOutput);
|
|
{
|
|
{
|
|
int i;
|
|
pylist = PyTuple_New(dofCount);
|
|
for (i = 0; i < dofCount; i++)
|
|
PyTuple_SetItem(pylist, i,
|
|
PyFloat_FromDouble(jointForcesOutput[i]));
|
|
}
|
|
}
|
|
}
|
|
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"Internal error in calculateInverseDynamics");
|
|
}
|
|
}
|
|
free(jointPositionsQ);
|
|
free(jointVelocitiesQdot);
|
|
free(jointAccelerations);
|
|
free(jointForcesOutput);
|
|
if (pylist) return pylist;
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"calculateInverseDynamics numJoints needs to be "
|
|
"positive and [joint positions], [joint velocities], "
|
|
"[joint accelerations] need to match the number of "
|
|
"joints.");
|
|
return NULL;
|
|
}
|
|
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"calculateInverseDynamics expects 4 arguments, body "
|
|
"index, [joint positions], [joint velocities], [joint "
|
|
"accelerations].");
|
|
return NULL;
|
|
}
|
|
} else {
|
|
PyErr_SetString(SpamError,
|
|
"calculateInverseDynamics expects 4 arguments, body index, "
|
|
"[joint positions], [joint velocities], [joint "
|
|
"accelerations].");
|
|
return NULL;
|
|
}
|
|
Py_INCREF(Py_None);
|
|
return Py_None;
|
|
}
|
|
|
|
static PyMethodDef SpamMethods[] = {
|
|
|
|
{"connect", pybullet_connectPhysicsServer, METH_VARARGS,
|
|
"Connect to an existing physics server (using shared memory by default)."},
|
|
|
|
{"disconnect", pybullet_disconnectPhysicsServer, METH_VARARGS,
|
|
"Disconnect from the physics server."},
|
|
|
|
{"resetSimulation", pybullet_resetSimulation, METH_VARARGS,
|
|
"Reset the simulation: remove all objects and start from an empty world."},
|
|
|
|
{"stepSimulation", pybullet_stepSimulation, METH_VARARGS,
|
|
"Step the simulation using forward dynamics."},
|
|
|
|
{"setGravity", pybullet_setGravity, METH_VARARGS,
|
|
"Set the gravity acceleration (x,y,z)."},
|
|
|
|
{"setTimeStep", pybullet_setTimeStep, METH_VARARGS,
|
|
"Set the amount of time to proceed at each call to stepSimulation. (unit "
|
|
"is seconds, typically range is 0.01 or 0.001)"},
|
|
|
|
|
|
|
|
{"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS,
|
|
"Set the amount of contact penetration Error Recovery Paramater "
|
|
"(ERP) in each time step. \
|
|
This is an tuning parameter to control resting contact stability. "
|
|
"This value depends on the time step."},
|
|
|
|
{ "setRealTimeSimulation", pybullet_setRealTimeSimulation, METH_VARARGS,
|
|
"Enable or disable real time simulation (using the real time clock,"
|
|
" RTC) in the physics server. Expects one integer argument, 0 or 1" },
|
|
|
|
{ "setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS,
|
|
"Set some internal physics engine parameter, such as cfm or erp etc." },
|
|
|
|
{ "setInternalSimFlags", pybullet_setInternalSimFlags, METH_VARARGS,
|
|
"This is for experimental purposes, use at own risk, magic may or not happen"},
|
|
|
|
{"loadURDF", (PyCFunction) pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS,
|
|
"Create a multibody by loading a URDF file."},
|
|
|
|
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
|
|
"Load multibodies from an SDF file."},
|
|
|
|
{ "loadBullet", pybullet_loadBullet, METH_VARARGS,
|
|
"Restore the full state of the world from a .bullet file." },
|
|
|
|
{ "saveBullet", pybullet_saveBullet, METH_VARARGS,
|
|
"Save the full state of the world to a .bullet file." },
|
|
|
|
{ "loadMJCF", pybullet_loadMJCF, METH_VARARGS,
|
|
"Load multibodies from an MJCF file." },
|
|
|
|
{"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS,
|
|
"Create a constraint between two bodies. Returns a (int) unique id, if successfull."
|
|
},
|
|
|
|
{"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS,
|
|
"Remove a constraint using its unique id."
|
|
},
|
|
|
|
{"saveWorld", pybullet_saveWorld, METH_VARARGS,
|
|
"Save a approximate Python file to reproduce the current state of the world: saveWorld"
|
|
"(filename). (very preliminary and approximately)"},
|
|
|
|
{"getNumBodies", pybullet_getNumBodies, METH_VARARGS,
|
|
"Get the number of bodies in the simulation."},
|
|
|
|
{"getBodyUniqueId", pybullet_getBodyUniqueId, METH_VARARGS,
|
|
"Get the unique id of the body, given a integer serial index in range [0.. number of bodies)."},
|
|
|
|
{"getBodyInfo", pybullet_getBodyInfo, METH_VARARGS,
|
|
"Get the body info, given a body unique id."},
|
|
|
|
{"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation,
|
|
METH_VARARGS,
|
|
"Get the world position and orientation of the base of the object. "
|
|
"(x,y,z) position vector and (x,y,z,w) quaternion orientation."},
|
|
|
|
{"resetBasePositionAndOrientation",
|
|
pybullet_resetBasePositionAndOrientation, METH_VARARGS,
|
|
"Reset the world position and orientation of the base of the object "
|
|
"instantaneously, not through physics simulation. (x,y,z) position vector "
|
|
"and (x,y,z,w) quaternion orientation."},
|
|
|
|
{ "getBaseVelocity", pybullet_getBaseVelocity,
|
|
METH_VARARGS,
|
|
"Get the linear and angular velocity of the base of the object "
|
|
" in world space coordinates. "
|
|
"(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." },
|
|
|
|
{ "resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS,
|
|
"Reset the linear and/or angular velocity of the base of the object "
|
|
" in world space coordinates. "
|
|
"linearVelocity (x,y,z) and angularVelocity (x,y,z)." },
|
|
|
|
|
|
|
|
{"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
|
|
"Get the number of joints for an object."},
|
|
|
|
{"getJointInfo", pybullet_getJointInfo, METH_VARARGS,
|
|
"Get the name and type info for a joint on a body."},
|
|
|
|
{"getJointState", pybullet_getJointState, METH_VARARGS,
|
|
"Get the state (position, velocity etc) for a joint on a body."},
|
|
|
|
{"getLinkState", pybullet_getLinkState, METH_VARARGS,
|
|
"Provides extra information such as the Cartesian world coordinates"
|
|
" center of mass (COM) of the link, relative to the world reference"
|
|
" frame."},
|
|
|
|
{"resetJointState", pybullet_resetJointState, METH_VARARGS,
|
|
"Reset the state (position, velocity etc) for a joint on a body "
|
|
"instantaneously, not through physics simulation."},
|
|
|
|
{"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS,
|
|
"Set a single joint motor control mode and desired target value. There is "
|
|
"no immediate state change, stepSimulation will process the motors."},
|
|
|
|
{"applyExternalForce", pybullet_applyExternalForce, METH_VARARGS,
|
|
"for objectUniqueId, linkIndex (-1 for base/root link), apply a force "
|
|
"[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or "
|
|
"FORCE_IN_WORLD_FRAME coordinates"},
|
|
|
|
{"applyExternalTorque", pybullet_applyExternalTorque, METH_VARARGS,
|
|
"for objectUniqueId, linkIndex (-1 for base/root link) apply a torque "
|
|
"[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or "
|
|
"TORQUE_IN_WORLD_FRAME coordinates"},
|
|
|
|
{"renderImage", pybullet_renderImageObsolete, METH_VARARGS,
|
|
"obsolete, please use getCameraImage and getViewProjectionMatrices instead"
|
|
},
|
|
|
|
{ "getCameraImage",(PyCFunction)pybullet_getCameraImage, METH_VARARGS| METH_KEYWORDS,
|
|
"Render an image (given the pixel resolution width, height, camera viewMatrix "
|
|
", projectionMatrix, lightDirection, lightColor, lightDistance, shadow, lightAmbientCoeff, lightDiffuseCoeff, and lightSpecularCoeff), and return the "
|
|
"8-8-8bit RGB pixel data and floating point depth values"
|
|
#ifdef PYBULLET_USE_NUMPY
|
|
" as NumPy arrays"
|
|
#endif
|
|
},
|
|
|
|
{ "computeViewMatrix", (PyCFunction)pybullet_computeViewMatrix, METH_VARARGS | METH_KEYWORDS,
|
|
"Compute a camera viewmatrix from camera eye, target position and up vector "
|
|
},
|
|
|
|
{ "computeViewMatrixFromYawPitchRoll",(PyCFunction)pybullet_computeViewMatrixFromYawPitchRoll, METH_VARARGS | METH_KEYWORDS,
|
|
"Compute a camera viewmatrix from camera eye, target position and up vector "
|
|
},
|
|
|
|
{ "computeProjectionMatrix", (PyCFunction)pybullet_computeProjectionMatrix, METH_VARARGS | METH_KEYWORDS,
|
|
"Compute a camera projection matrix from screen left/right/bottom/top/near/far values"
|
|
},
|
|
|
|
{ "computeProjectionMatrixFOV", (PyCFunction)pybullet_computeProjectionMatrixFOV, METH_VARARGS | METH_KEYWORDS,
|
|
"Compute a camera projection matrix from fov, aspect ratio, near, far values"
|
|
},
|
|
|
|
{"getContactPoints", (PyCFunction)pybullet_getContactPointData, METH_VARARGS | METH_KEYWORDS,
|
|
"Return existing contact points after the stepSimulation command. "
|
|
"Optional arguments one or two object unique "
|
|
"ids, that need to be involved in the contact."},
|
|
|
|
{"getClosestPoints", (PyCFunction)pybullet_getClosestPointData, METH_VARARGS | METH_KEYWORDS,
|
|
"Compute the closest points between two objects, if the distance is below a given threshold."
|
|
"Input is two objects unique ids and distance threshold."
|
|
},
|
|
|
|
{ "getOverlappingObjects", (PyCFunction)pybullet_getOverlappingObjects, METH_VARARGS | METH_KEYWORDS,
|
|
"Return all the objects that have overlap with a given "
|
|
"axis-aligned bounding box volume (AABB)."
|
|
"Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]."
|
|
},
|
|
|
|
|
|
{ "addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS,
|
|
"Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. "
|
|
"A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."
|
|
},
|
|
|
|
|
|
{ "addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS,
|
|
"Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds "
|
|
"A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."
|
|
},
|
|
|
|
{ "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS,
|
|
"remove a user debug draw item, giving its unique id"
|
|
},
|
|
|
|
|
|
{ "removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS,
|
|
"remove all user debug draw items"
|
|
},
|
|
|
|
{ "setDebugObjectColor", (PyCFunction)pybullet_setDebugObjectColor, METH_VARARGS | METH_KEYWORDS,
|
|
"Override the wireframe debug drawing color for a particular object unique id / link index."
|
|
"If you ommit the color, the custom color will be removed."
|
|
},
|
|
|
|
|
|
{"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,
|
|
"Return the visual shape information for one object." },
|
|
|
|
{"resetVisualShapeData", pybullet_resetVisualShapeData, METH_VARARGS,
|
|
"Reset part of the visual shape information for one object." },
|
|
|
|
{"loadTexture", pybullet_loadTexture, METH_VARARGS,
|
|
"Load texture file." },
|
|
|
|
{"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
|
|
"Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to "
|
|
"quaternion [x,y,z,w]"},
|
|
|
|
{"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS,
|
|
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF "
|
|
"convention"},
|
|
|
|
{"calculateInverseDynamics", pybullet_calculateInverseDynamics,
|
|
METH_VARARGS,
|
|
"Given an object id, joint positions, joint velocities and joint "
|
|
"accelerations, compute the joint forces using Inverse Dynamics"},
|
|
|
|
{"calculateInverseKinematicsKuka", pybullet_calculateInverseKinematicsKuka,
|
|
METH_VARARGS,
|
|
"Experimental, KUKA IIWA only: Given an object id, "
|
|
"current joint positions and target position"
|
|
" for the end effector,"
|
|
"compute the inverse kinematics and return the new joint state"},
|
|
|
|
// todo(erwincoumans)
|
|
// saveSnapshot
|
|
// loadSnapshot
|
|
// raycast info
|
|
// object names
|
|
// collision query
|
|
|
|
{NULL, NULL, 0, NULL} /* Sentinel */
|
|
};
|
|
|
|
#if PY_MAJOR_VERSION >= 3
|
|
static struct PyModuleDef moduledef = {
|
|
PyModuleDef_HEAD_INIT, "pybullet", /* m_name */
|
|
"Python bindings for Bullet Physics Robotics API (also known as Shared "
|
|
"Memory API)", /* m_doc */
|
|
-1, /* m_size */
|
|
SpamMethods, /* m_methods */
|
|
NULL, /* m_reload */
|
|
NULL, /* m_traverse */
|
|
NULL, /* m_clear */
|
|
NULL, /* m_free */
|
|
};
|
|
#endif
|
|
|
|
PyMODINIT_FUNC
|
|
#if PY_MAJOR_VERSION >= 3
|
|
PyInit_pybullet(void)
|
|
#else
|
|
initpybullet(void)
|
|
#endif
|
|
{
|
|
|
|
PyObject* m;
|
|
#if PY_MAJOR_VERSION >= 3
|
|
m = PyModule_Create(&moduledef);
|
|
#else
|
|
m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet");
|
|
#endif
|
|
|
|
#if PY_MAJOR_VERSION >= 3
|
|
if (m == NULL) return m;
|
|
#else
|
|
if (m == NULL) return;
|
|
#endif
|
|
|
|
PyModule_AddIntConstant(m, "SHARED_MEMORY",
|
|
eCONNECT_SHARED_MEMORY); // user read
|
|
PyModule_AddIntConstant(m, "DIRECT", eCONNECT_DIRECT); // user read
|
|
PyModule_AddIntConstant(m, "GUI", eCONNECT_GUI); // user read
|
|
PyModule_AddIntConstant(m, "UDP", eCONNECT_UDP); // user read
|
|
|
|
|
|
PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
|
|
PyModule_AddIntConstant(m, "VELOCITY_CONTROL",
|
|
CONTROL_MODE_VELOCITY); // user read
|
|
PyModule_AddIntConstant(m, "POSITION_CONTROL",
|
|
CONTROL_MODE_POSITION_VELOCITY_PD); // user read
|
|
|
|
PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME);
|
|
PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME);
|
|
|
|
PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS);
|
|
PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS);
|
|
|
|
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
|
|
Py_INCREF(SpamError);
|
|
PyModule_AddObject(m, "error", SpamError);
|
|
|
|
#ifdef PYBULLET_USE_NUMPY
|
|
// Initialize numpy array.
|
|
import_array();
|
|
#endif //PYBULLET_USE_NUMPY
|
|
|
|
|
|
#if PY_MAJOR_VERSION >= 3
|
|
return m;
|
|
#endif
|
|
}
|