AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity
f7522873f9
bullet3/examples/pybullet/pybullet.c
Erwin Coumans (Google) f7522873f9 fix compile issue on Linux
2016-09-01 22:11:07 -07:00

1992 lines
59 KiB
C
Raw Blame History

#include "../SharedMemory/PhysicsClientC_API.h"
#include "../SharedMemory/PhysicsDirectC_API.h"
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
#ifdef __APPLE__
#include <Python/Python.h>
#else
#include <Python.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,
};
static PyObject *SpamError;
static b3PhysicsClientHandle sm=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;
if (!PyArg_ParseTuple(args, "i", &method))
{
PyErr_SetString(SpamError, "connectPhysicsServer expected argument eCONNECT_GUI, eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY");
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(SHARED_MEMORY_KEY);
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;
}
// Load a URDF file indicating the links and joints of an object
// 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)
{
int size= PySequence_Size(args);
int bodyIndex = -1;
const char* urdfFileName="";
float startPosX =0;
float startPosY =0;
float startPosZ = 0;
float startOrnX = 0;
float startOrnY = 0;
float startOrnZ = 0;
float startOrnW = 1;
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size==1)
{
if (!PyArg_ParseTuple(args, "s", &urdfFileName))
return NULL;
}
if (size == 4)
{
if (!PyArg_ParseTuple(args, "sfff", &urdfFileName,
&startPosX,&startPosY,&startPosZ))
return NULL;
}
if (size==8)
{
if (!PyArg_ParseTuple(args, "sfffffff", &urdfFileName,
&startPosX,&startPosY,&startPosZ,
&startOrnX,&startOrnY,&startOrnZ, &startOrnW))
return NULL;
}
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 );
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 float pybullet_internalGetFloatFromSequence(PyObject* seq, int index)
{
float v = 0.f;
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;
}
#define MAX_SDF_BODIES 512
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;
double targetVelocity=0;
double maxForce=100000;
double appliedForce = 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;
// 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;
//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;
double targetValue = 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;
}
// 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;
}
{
float gravX=0;
float gravY=0;
float gravZ=-10;
int ret;
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
if (!PyArg_ParseTuple(args, "fff", &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;
}
// 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[3])
{
basePosition[0] = 0.;
basePosition[1] = 0.;
basePosition[2] = 0.;
baseOrientation[0] = 0.;
baseOrientation[1] = 0.;
baseOrientation[2] = 0.;
baseOrientation[3] = 1.;
{
{
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[];
int i;
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 (#joints/links exceeds maximum?).");
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;
}
}
// 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;
}
// 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;
PyObject *item;
struct b3JointInfo info;
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 i, 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 =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
status_type = b3GetStatusType(status_handle);
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getBasePositionAndOrientation 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;
}
// 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");
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:
// matrix - float[16] which will be set by values from objMat
static int pybullet_internalSetVector(PyObject* objMat, float vector[3])
{
int i, len;
PyObject* seq;
seq = PySequence_Fast(objMat, "expected a sequence");
len = PySequence_Size(objMat);
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;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args)
{
int size= PySequence_Size(args);
int objectUniqueIdA = -1;
int objectUniqueIdB = -1;
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* pyResultList=0;
if (size==1)
{
if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA))
{
PyErr_SetString(SpamError, "Error parsing object unique id");
return NULL;
}
}
if (size==2)
{
if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA,&objectUniqueIdB))
{
PyErr_SetString(SpamError, "Error parsing object unique id");
return NULL;
}
}
commandHandle = b3InitRequestContactPointInformation(sm);
b3SetContactFilterBodyA(commandHandle,objectUniqueIdA);
b3SetContactFilterBodyB(commandHandle,objectUniqueIdB);
b3SubmitClientCommand(sm, commandHandle);
int i;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
/*
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5-6-7 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
8-9-10 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
11-12-13 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
14 double m_contactDistance;//negative number is penetration, positive is distance.
15 double m_normalForce;
*/
b3GetContactPointInformation(sm, &contactPointData);
pyResultList = PyTuple_New(contactPointData.m_numContactPoints);
for (i=0;i<contactPointData.m_numContactPoints;i++)
{
PyObject* contactObList = PyTuple_New(16);//see above 16 fields
PyObject* item;
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags);
PyTuple_SetItem(contactObList,0,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_bodyUniqueIdA);
PyTuple_SetItem(contactObList,1,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_bodyUniqueIdB);
PyTuple_SetItem(contactObList,2,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA);
PyTuple_SetItem(contactObList,3,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB);
PyTuple_SetItem(contactObList,4,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[0]);
PyTuple_SetItem(contactObList,5,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[1]);
PyTuple_SetItem(contactObList,6,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[2]);
PyTuple_SetItem(contactObList,7,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[0]);
PyTuple_SetItem(contactObList,8,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[1]);
PyTuple_SetItem(contactObList,9,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[2]);
PyTuple_SetItem(contactObList,10,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]);
PyTuple_SetItem(contactObList,11,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]);
PyTuple_SetItem(contactObList,12,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]);
PyTuple_SetItem(contactObList,13,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactDistance);
PyTuple_SetItem(contactObList,14,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_normalForce);
PyTuple_SetItem(contactObList,15,item);
PyTuple_SetItem(pyResultList, i, contactObList);
}
return pyResultList;
}
Py_INCREF(Py_None);
return Py_None;
}
// 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_renderImage(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
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;
}
}
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};
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;
}
///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)"},
{ "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" },
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
"Create a multibody by loading a URDF file."},
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
"Load multibodies from an SDF file."},
{"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."},
{"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."},
{"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_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width, height, camera view matrix, projection matrix, near, and far values), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{"getContactPointData", pybullet_getContactPointData, METH_VARARGS,
"Return the contact point information for all or some of pairwise object-object collisions. Optional arguments one or two object unique ids, that need to be involved in the contact."},
{"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" },
//todo(erwincoumans)
//saveSnapshot
//loadSnapshot
////todo(erwincoumans)
//collision info
//raycast info
//applyBaseForce
//applyLinkForce
{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, "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);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError);
#if PY_MAJOR_VERSION >= 3
return m;
#endif
}
Reference in New Issue View Git Blame Copy Permalink