2017-03-30 20:56:37 +00:00
2017-04-04 19:47:34 +00:00
2016-04-30 18:18:54 +00:00
# include "../SharedMemory/PhysicsClientC_API.h"
2016-05-10 07:57:54 +00:00
# include "../SharedMemory/PhysicsDirectC_API.h"
2016-06-17 01:46:34 +00:00
# include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
2016-11-04 20:15:10 +00:00
# ifdef BT_ENABLE_ENET
# include "../SharedMemory/PhysicsClientUDP_C_API.h"
2017-03-13 23:06:52 +00:00
# endif //BT_ENABLE_ENET
2016-09-11 10:09:51 +00:00
2017-02-21 04:34:05 +00:00
# ifdef BT_ENABLE_CLSOCKET
# include "../SharedMemory/PhysicsClientTCP_C_API.h"
2017-03-13 23:06:52 +00:00
# endif //BT_ENABLE_CLSOCKET
2017-02-21 04:34:05 +00:00
2017-04-11 19:17:08 +00:00
# if defined(__APPLE__) && (!defined(B3_NO_PYTHON_FRAMEWORK))
2016-04-30 18:18:54 +00:00
# include <Python/Python.h>
# else
# include <Python.h>
# endif
2017-04-11 19:17:08 +00:00
# ifdef B3_DUMP_PYTHON_VERSION
# define B3_VALUE_TO_STRING(x) #x
# define B3_VALUE(x) B3_VALUE_TO_STRING(x)
# define B3_VAR_NAME_VALUE(var) #var "=" B3_VALUE(var)
# pragma message(B3_VAR_NAME_VALUE(PY_MAJOR_VERSION))
# pragma message(B3_VAR_NAME_VALUE(PY_MINOR_VERSION))
# endif
2016-09-11 10:09:51 +00:00
# ifdef PYBULLET_USE_NUMPY
# include <numpy/arrayobject.h>
# endif
2017-04-11 19:17:08 +00:00
2016-06-23 05:10:00 +00:00
# if PY_MAJOR_VERSION >= 3
# define PyInt_FromLong PyLong_FromLong
# define PyString_FromString PyBytes_FromString
2016-09-08 18:12:58 +00:00
# endif
2016-06-23 05:10:00 +00:00
2016-09-08 18:12:58 +00:00
static PyObject * SpamError ;
2016-12-28 04:25:52 +00:00
# define MAX_PHYSICS_CLIENTS 1024
2016-12-29 05:51:54 +00:00
static b3PhysicsClientHandle sPhysicsClients1 [ MAX_PHYSICS_CLIENTS ] = { 0 } ;
2017-01-21 22:30:37 +00:00
static int sPhysicsClientsGUI [ MAX_PHYSICS_CLIENTS ] = { 0 } ;
2017-03-13 23:06:52 +00:00
static int sNumPhysicsClients = 0 ;
2016-05-03 19:59:21 +00:00
2016-12-29 05:51:54 +00:00
b3PhysicsClientHandle getPhysicsClient ( int physicsClientId )
{
b3PhysicsClientHandle sm ;
2017-03-13 23:06:52 +00:00
if ( ( physicsClientId < 0 ) | | ( physicsClientId > = MAX_PHYSICS_CLIENTS ) | | ( 0 = = sPhysicsClients1 [ physicsClientId ] ) )
2016-12-29 05:51:54 +00:00
{
return 0 ;
}
sm = sPhysicsClients1 [ physicsClientId ] ;
if ( sm )
{
if ( b3CanSubmitCommand ( sm ) )
{
return sm ;
}
//broken connection?
b3DisconnectSharedMemory ( sm ) ;
sPhysicsClients1 [ physicsClientId ] = 0 ;
2017-01-21 22:30:37 +00:00
sPhysicsClientsGUI [ physicsClientId ] = 0 ;
2016-12-29 05:51:54 +00:00
sNumPhysicsClients - - ;
}
return 0 ;
}
2016-11-28 23:11:34 +00:00
2017-03-13 23:06:52 +00:00
static double pybullet_internalGetFloatFromSequence ( PyObject * seq , int index )
{
2016-11-28 23:11:34 +00:00
double v = 0.0 ;
PyObject * item ;
2017-03-13 23:06:52 +00:00
if ( PyList_Check ( seq ) )
{
2016-11-28 23:11:34 +00:00
item = PyList_GET_ITEM ( seq , index ) ;
v = PyFloat_AsDouble ( item ) ;
}
2017-03-13 23:06:52 +00:00
else
{
2016-11-28 23:11:34 +00:00
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
2017-03-13 23:06:52 +00:00
static int pybullet_internalSetMatrix ( PyObject * objMat , float matrix [ 16 ] )
{
2016-11-28 23:11:34 +00:00
int i , len ;
PyObject * seq ;
seq = PySequence_Fast ( objMat , " expected a sequence " ) ;
if ( seq )
{
len = PySequence_Size ( objMat ) ;
2017-03-13 23:06:52 +00:00
if ( len = = 16 )
{
for ( i = 0 ; i < len ; i + + )
{
2016-11-28 23:11:34 +00:00
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
2017-03-13 23:06:52 +00:00
static int pybullet_internalSetVector ( PyObject * objVec , float vector [ 3 ] )
{
2016-11-28 23:11:34 +00:00
int i , len ;
PyObject * seq = 0 ;
if ( objVec = = NULL )
return 0 ;
seq = PySequence_Fast ( objVec , " expected a sequence " ) ;
if ( seq )
{
len = PySequence_Size ( objVec ) ;
2017-03-13 23:06:52 +00:00
if ( len = = 3 )
{
for ( i = 0 ; i < len ; i + + )
{
2016-11-28 23:11:34 +00:00
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
2017-03-13 23:06:52 +00:00
static int pybullet_internalSetVectord ( PyObject * obVec , double vector [ 3 ] )
{
2016-11-28 23:11:34 +00:00
int i , len ;
PyObject * seq ;
if ( obVec = = NULL )
return 0 ;
seq = PySequence_Fast ( obVec , " expected a sequence " ) ;
if ( seq )
{
len = PySequence_Size ( obVec ) ;
2017-03-13 23:06:52 +00:00
if ( len = = 3 )
{
for ( i = 0 ; i < len ; i + + )
{
2016-11-28 23:11:34 +00:00
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
2017-03-13 23:06:52 +00:00
static int pybullet_internalSetVector4d ( PyObject * obVec , double vector [ 4 ] )
{
2016-11-28 23:11:34 +00:00
int i , len ;
PyObject * seq ;
if ( obVec = = NULL )
return 0 ;
seq = PySequence_Fast ( obVec , " expected a sequence " ) ;
len = PySequence_Size ( obVec ) ;
2017-03-13 23:06:52 +00:00
if ( len = = 4 )
{
for ( i = 0 ; i < len ; i + + )
{
2016-11-28 23:11:34 +00:00
vector [ i ] = pybullet_internalGetFloatFromSequence ( seq , i ) ;
}
Py_DECREF ( seq ) ;
return 1 ;
}
Py_DECREF ( seq ) ;
return 0 ;
}
2016-06-10 22:07:41 +00:00
// Step through one timestep of the simulation
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_stepSimulation ( PyObject * self , PyObject * args , PyObject * keywds )
2016-12-28 04:25:52 +00:00
{
int physicsClientId = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " physicsClientId " , NULL } ;
b3PhysicsClientHandle sm = 0 ;
2016-04-30 18:18:54 +00:00
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |i " , kwlist , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-04-30 18:18:54 +00:00
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
if ( b3CanSubmitCommand ( sm ) )
{
statusHandle = b3SubmitClientCommandAndWaitStatus (
sm , b3InitStepSimulationCommand ( sm ) ) ;
statusType = b3GetStatusType ( statusHandle ) ;
}
2016-12-28 04:25:52 +00:00
}
2017-03-13 23:06:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_connectPhysicsServer ( PyObject * self , PyObject * args , PyObject * keywds )
{
int freeIndex = - 1 ;
int method = eCONNECT_GUI ;
int i ;
2017-03-30 18:01:33 +00:00
char * options = " " ;
2017-03-13 23:06:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2016-11-05 00:06:55 +00:00
2017-03-13 23:06:52 +00:00
if ( sNumPhysicsClients > = MAX_PHYSICS_CLIENTS )
2016-11-05 00:06:55 +00:00
{
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError ,
" Exceeding maximum number of physics connections. " ) ;
return NULL ;
2016-11-05 00:06:55 +00:00
}
2017-01-21 22:30:37 +00:00
{
2017-03-13 23:06:52 +00:00
int key = SHARED_MEMORY_KEY ;
int udpPort = 1234 ;
int tcpPort = 6667 ;
const char * hostName = " localhost " ;
2017-03-30 18:01:33 +00:00
static char * kwlist1 [ ] = { " method " , " key " , " options " , NULL } ;
static char * kwlist2 [ ] = { " method " , " hostName " , " port " , " options " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|is " , kwlist1 , & method , & key , & options ) )
2017-01-21 22:30:37 +00:00
{
2017-03-30 18:01:33 +00:00
int port = - 1 ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|sis " , kwlist2 , & method , & hostName , & port , & options ) )
2017-01-21 22:30:37 +00:00
{
return NULL ;
2017-03-30 18:01:33 +00:00
} else
{
PyErr_Clear ( ) ;
if ( port > = 0 )
{
udpPort = port ;
tcpPort = port ;
}
2017-01-21 22:30:37 +00:00
}
}
2017-03-13 23:06:52 +00:00
//Only one local in-process GUI connection allowed.
if ( method = = eCONNECT_GUI )
2016-11-05 00:06:55 +00:00
{
2017-03-13 23:06:52 +00:00
int i ;
for ( i = 0 ; i < MAX_PHYSICS_CLIENTS ; i + + )
2016-11-05 00:06:55 +00:00
{
2017-03-13 23:06:52 +00:00
if ( sPhysicsClientsGUI [ i ] = = eCONNECT_GUI )
{
PyErr_SetString ( SpamError ,
" Only one local in-process GUI connection allowed. Use DIRECT connection mode or start a separate GUI physics server (ExampleBrowser, App_SharedMemoryPhysics_GUI, App_SharedMemoryPhysics_VR) and connect over SHARED_MEMORY, UDP or TCP instead. " ) ;
return NULL ;
}
}
}
2016-11-05 00:06:55 +00:00
2017-03-13 23:06:52 +00:00
switch ( method )
{
case eCONNECT_GUI :
{
2017-03-30 18:01:33 +00:00
int argc = 2 ;
char * argv [ 2 ] = { " unused " , options } ;
2016-05-10 07:57:54 +00:00
# ifdef __APPLE__
2017-03-13 23:06:52 +00:00
sm = b3CreateInProcessPhysicsServerAndConnectMainThread ( argc , argv ) ;
2016-05-10 07:57:54 +00:00
# else
2017-03-13 23:06:52 +00:00
sm = b3CreateInProcessPhysicsServerAndConnect ( argc , argv ) ;
2016-05-10 07:57:54 +00:00
# endif
2017-03-13 23:06:52 +00:00
break ;
}
case eCONNECT_DIRECT :
{
sm = b3ConnectPhysicsDirect ( ) ;
break ;
}
case eCONNECT_SHARED_MEMORY :
{
sm = b3ConnectSharedMemory ( key ) ;
break ;
}
case eCONNECT_UDP :
{
2016-11-04 20:15:10 +00:00
# ifdef BT_ENABLE_ENET
2017-03-13 23:06:52 +00:00
sm = b3ConnectPhysicsUDP ( hostName , udpPort ) ;
2016-11-04 20:15:10 +00:00
# else
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " UDP is not enabled in this pybullet build " ) ;
return NULL ;
# endif //BT_ENABLE_ENET
2016-11-04 20:15:10 +00:00
2017-03-13 23:06:52 +00:00
break ;
}
case eCONNECT_TCP :
{
2017-02-21 04:34:05 +00:00
# ifdef BT_ENABLE_CLSOCKET
2017-03-13 23:06:52 +00:00
sm = b3ConnectPhysicsTCP ( hostName , tcpPort ) ;
2017-02-21 04:34:05 +00:00
# else
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " TCP is not enabled in this pybullet build " ) ;
return NULL ;
# endif //BT_ENABLE_CLSOCKET
break ;
}
default :
{
PyErr_SetString ( SpamError , " connectPhysicsServer unexpected argument " ) ;
return NULL ;
}
} ;
}
if ( sm & & b3CanSubmitCommand ( sm ) )
{
for ( i = 0 ; i < MAX_PHYSICS_CLIENTS ; i + + )
{
if ( sPhysicsClients1 [ i ] = = 0 )
{
freeIndex = i ;
break ;
}
}
if ( freeIndex > = 0 )
{
b3SharedMemoryCommandHandle command ;
2017-01-21 02:13:24 +00:00
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
2017-03-13 23:06:52 +00:00
sPhysicsClients1 [ freeIndex ] = sm ;
sPhysicsClientsGUI [ freeIndex ] = method ;
sNumPhysicsClients + + ;
2017-01-21 02:13:24 +00:00
2017-03-13 23:06:52 +00:00
command = b3InitSyncBodyInfoCommand ( sm ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
2017-01-21 02:13:24 +00:00
statusType = b3GetStatusType ( statusHandle ) ;
#if 0
2017-02-22 17:33:30 +00:00
if ( statusType ! = CMD_BODY_INFO_COMPLETED )
{
PyErr_SetString ( SpamError , " b3InitSyncBodyInfoCommand failed. " ) ;
return NULL ;
}
2017-01-21 02:13:24 +00:00
# endif
2017-03-13 23:06:52 +00:00
}
}
return PyInt_FromLong ( freeIndex ) ;
2016-09-08 18:12:58 +00:00
}
static PyObject * pybullet_disconnectPhysicsServer ( PyObject * self ,
2017-03-13 23:06:52 +00:00
PyObject * args ,
PyObject * keywds )
{
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
2017-03-13 23:06:52 +00:00
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |i " , kwlist , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
{
b3DisconnectSharedMemory ( sm ) ;
sm = 0 ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
sPhysicsClients1 [ physicsClientId ] = 0 ;
sPhysicsClientsGUI [ physicsClientId ] = 0 ;
sNumPhysicsClients - - ;
2016-12-28 04:25:52 +00:00
2017-03-13 23:06:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-05-03 19:59:21 +00:00
}
2017-04-05 22:21:26 +00:00
///to avoid memory leaks, disconnect all physics servers explicitly
void b3pybulletExitFunc ( void )
2017-03-30 20:50:02 +00:00
{
int i ;
for ( i = 0 ; i < MAX_PHYSICS_CLIENTS ; i + + )
{
if ( sPhysicsClients1 [ i ] )
{
b3DisconnectSharedMemory ( sPhysicsClients1 [ i ] ) ;
sPhysicsClients1 [ i ] = 0 ;
sNumPhysicsClients - - ;
}
}
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_saveWorld ( PyObject * self , PyObject * args , PyObject * keywds )
{
2016-10-13 06:03:36 +00:00
const char * worldFileName = " " ;
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
int physicsClientId = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " worldFileName " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " s|i " , kwlist , & worldFileName , & physicsClientId ) )
2016-10-13 06:03:36 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-10-13 06:03:36 +00:00
{
2017-03-13 23:06:52 +00:00
b3SharedMemoryCommandHandle command ;
2016-10-13 06:03:36 +00:00
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
command = b3SaveWorldCommandInit ( sm , worldFileName ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
statusType = b3GetStatusType ( statusHandle ) ;
2017-03-13 23:06:52 +00:00
if ( statusType ! = CMD_SAVE_WORLD_COMPLETED )
{
2016-10-13 06:03:36 +00:00
PyErr_SetString ( SpamError , " saveWorld command execution failed. " ) ;
return NULL ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
2017-03-13 23:06:52 +00:00
}
2016-10-13 06:03:36 +00:00
PyErr_SetString ( SpamError , " Cannot execute saveWorld command. " ) ;
return NULL ;
2016-11-11 22:44:50 +00:00
}
2016-11-14 15:39:34 +00:00
# define MAX_SDF_BODIES 512
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_loadBullet ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-11 22:44:50 +00:00
{
const char * bulletFileName = " " ;
2016-11-12 02:07:42 +00:00
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
b3SharedMemoryCommandHandle command ;
2017-03-13 23:06:52 +00:00
int i , numBodies ;
2016-11-14 15:39:34 +00:00
int bodyIndicesOut [ MAX_SDF_BODIES ] ;
2016-12-28 04:25:52 +00:00
PyObject * pylist = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bulletFileName " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " s|i " , kwlist , & bulletFileName , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
2016-11-11 22:44:50 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
2016-11-12 02:07:42 +00:00
return NULL ;
2016-11-11 22:44:50 +00:00
}
2016-11-12 02:07:42 +00:00
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 ;
}
2016-11-14 15:39:34 +00:00
numBodies =
2017-03-13 23:06:52 +00:00
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 ;
2016-11-11 22:44:50 +00:00
}
2016-12-28 04:25:52 +00:00
static PyObject * pybullet_saveBullet ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-11 22:44:50 +00:00
{
const char * bulletFileName = " " ;
2016-11-12 02:07:42 +00:00
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
b3SharedMemoryCommandHandle command ;
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2016-11-11 22:44:50 +00:00
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bulletFileName " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " s|i " , kwlist , & bulletFileName , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
2016-11-11 22:44:50 +00:00
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
2016-11-11 22:44:50 +00:00
}
2016-12-28 04:25:52 +00:00
2016-11-12 02:07:42 +00:00
command = b3SaveBulletCommandInit ( sm , bulletFileName ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
2016-11-14 22:08:05 +00:00
statusType = b3GetStatusType ( statusHandle ) ;
if ( statusType ! = CMD_BULLET_SAVING_COMPLETED )
2016-11-12 02:07:42 +00:00
{
PyErr_SetString ( SpamError , " Couldn't save .bullet file. " ) ;
return NULL ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-11-11 22:44:50 +00:00
}
2016-10-13 06:03:36 +00:00
2016-12-28 04:25:52 +00:00
static PyObject * pybullet_loadMJCF ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-11 22:44:50 +00:00
{
2016-12-28 04:25:52 +00:00
const char * mjcfFileName = " " ;
2016-11-12 02:07:42 +00:00
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
b3SharedMemoryCommandHandle command ;
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2016-12-31 22:43:15 +00:00
int numBodies = 0 ;
int i ;
int bodyIndicesOut [ MAX_SDF_BODIES ] ;
PyObject * pylist = 0 ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
2017-05-10 22:01:25 +00:00
int flags = - 1 ;
static char * kwlist [ ] = { " mjcfFileName " , " flags " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " s|ii " , kwlist , & mjcfFileName , & flags , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
2016-11-11 22:44:50 +00:00
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
2016-11-11 22:44:50 +00:00
}
2016-12-28 04:25:52 +00:00
command = b3LoadMJCFCommandInit ( sm , mjcfFileName ) ;
2017-05-10 22:01:25 +00:00
if ( flags > = 0 )
{
b3LoadMJCFCommandSetFlags ( command , flags ) ;
}
2016-11-12 02:07:42 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
2016-11-14 22:08:05 +00:00
statusType = b3GetStatusType ( statusHandle ) ;
if ( statusType ! = CMD_MJCF_LOADING_COMPLETED )
2016-11-12 02:07:42 +00:00
{
PyErr_SetString ( SpamError , " Couldn't load .mjcf file. " ) ;
return NULL ;
}
2016-12-31 22:43:15 +00:00
2017-03-13 23:06:52 +00:00
numBodies =
b3GetStatusBodyIndices ( statusHandle , bodyIndicesOut , MAX_SDF_BODIES ) ;
if ( numBodies > MAX_SDF_BODIES )
{
PyErr_SetString ( SpamError , " SDF exceeds body capacity " ) ;
return NULL ;
}
2016-12-31 22:43:15 +00:00
2017-03-13 23:06:52 +00:00
pylist = PyTuple_New ( numBodies ) ;
2016-12-31 22:43:15 +00:00
2017-03-13 23:06:52 +00:00
if ( numBodies > 0 & & numBodies < = MAX_SDF_BODIES )
{
for ( i = 0 ; i < numBodies ; i + + )
{
PyTuple_SetItem ( pylist , i , PyInt_FromLong ( bodyIndicesOut [ i ] ) ) ;
}
}
return pylist ;
2016-11-11 22:44:50 +00:00
}
2017-05-09 17:44:33 +00:00
static PyObject * pybullet_changeDynamicsInfo ( PyObject * self , PyObject * args , PyObject * keywds )
2017-05-02 05:18:54 +00:00
{
int bodyUniqueId = - 1 ;
int linkIndex = - 2 ;
double mass = - 1 ;
2017-05-04 04:47:53 +00:00
double lateralFriction = - 1 ;
2017-05-02 05:18:54 +00:00
b3PhysicsClientHandle sm = 0 ;
int physicsClientId = 0 ;
2017-05-04 04:47:53 +00:00
static char * kwlist [ ] = { " bodyUniqueId " , " linkIndex " , " mass " , " lateralFriction " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ii|ddi " , kwlist , & bodyUniqueId , & linkIndex , & mass , & lateralFriction , & physicsClientId ) )
2017-05-02 05:18:54 +00:00
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
2017-05-09 17:44:33 +00:00
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo ( sm ) ;
2017-05-02 05:18:54 +00:00
b3SharedMemoryStatusHandle statusHandle ;
if ( mass > = 0 )
{
2017-05-09 17:44:33 +00:00
b3ChangeDynamicsInfoSetMass ( command , bodyUniqueId , linkIndex , mass ) ;
2017-05-02 05:18:54 +00:00
}
2017-05-04 04:47:53 +00:00
if ( lateralFriction > = 0 )
{
2017-05-09 17:44:33 +00:00
b3ChangeDynamicsInfoSetLateralFriction ( command , bodyUniqueId , linkIndex , lateralFriction ) ;
2017-05-04 04:47:53 +00:00
}
2017-05-02 05:18:54 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-05-09 17:31:28 +00:00
static PyObject * pybullet_getDynamicsInfo ( PyObject * self , PyObject * args , PyObject * keywds )
2017-05-08 05:41:05 +00:00
{
{
int bodyUniqueId = - 1 ;
int linkIndex = - 2 ;
b3PhysicsClientHandle sm = 0 ;
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bodyUniqueId " , " linkIndex " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ii|i " , kwlist , & bodyUniqueId , & linkIndex , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
int status_type = 0 ;
b3SharedMemoryCommandHandle cmd_handle ;
b3SharedMemoryStatusHandle status_handle ;
if ( bodyUniqueId < 0 )
{
2017-05-09 17:31:28 +00:00
PyErr_SetString ( SpamError , " getDynamicsInfo failed; invalid bodyUniqueId " ) ;
2017-05-08 05:41:05 +00:00
return NULL ;
}
if ( linkIndex < - 1 )
{
2017-05-09 17:31:28 +00:00
PyErr_SetString ( SpamError , " getDynamicsInfo failed; invalid linkIndex " ) ;
2017-05-08 05:41:05 +00:00
return NULL ;
}
2017-05-09 17:31:28 +00:00
cmd_handle = b3GetDynamicsInfoCommandInit ( sm , bodyUniqueId , linkIndex ) ;
2017-05-08 05:41:05 +00:00
status_handle = b3SubmitClientCommandAndWaitStatus ( sm , cmd_handle ) ;
status_type = b3GetStatusType ( status_handle ) ;
2017-05-09 17:31:28 +00:00
if ( status_type ! = CMD_GET_DYNAMICS_INFO_COMPLETED )
2017-05-08 05:41:05 +00:00
{
2017-05-09 17:31:28 +00:00
PyErr_SetString ( SpamError , " getDynamicsInfo failed; invalid return status " ) ;
2017-05-08 05:41:05 +00:00
return NULL ;
}
2017-05-09 17:31:28 +00:00
struct b3DynamicsInfo info ;
if ( b3GetDynamicsInfo ( status_handle , & info ) )
2017-05-08 05:41:05 +00:00
{
2017-05-09 17:31:28 +00:00
PyObject * pyDynamicsInfo = PyTuple_New ( 2 ) ;
PyTuple_SetItem ( pyDynamicsInfo , 0 , PyFloat_FromDouble ( info . m_mass ) ) ;
PyTuple_SetItem ( pyDynamicsInfo , 1 , PyFloat_FromDouble ( info . m_lateralFrictionCoeff ) ) ;
return pyDynamicsInfo ;
2017-05-08 05:41:05 +00:00
}
}
}
2017-05-09 17:31:28 +00:00
PyErr_SetString ( SpamError , " Couldn't get dynamics info " ) ;
2017-05-08 05:41:05 +00:00
return NULL ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_setPhysicsEngineParameter ( PyObject * self , PyObject * args , PyObject * keywds )
2016-12-01 06:24:20 +00:00
{
double fixedTimeStep = - 1 ;
int numSolverIterations = - 1 ;
int useSplitImpulse = - 1 ;
double splitImpulsePenetrationThreshold = - 1 ;
2016-12-02 01:54:52 +00:00
int numSubSteps = - 1 ;
2017-01-17 02:17:18 +00:00
int collisionFilterMode = - 1 ;
2017-02-16 22:19:09 +00:00
double contactBreakingThreshold = - 1 ;
2017-03-04 20:59:21 +00:00
int maxNumCmdPer1ms = - 2 ;
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2016-12-01 06:24:20 +00:00
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " fixedTimeStep " , " numSolverIterations " , " useSplitImpulse " , " splitImpulsePenetrationThreshold " , " numSubSteps " , " collisionFilterMode " , " contactBreakingThreshold " , " maxNumCmdPer1ms " , " physicsClientId " , NULL } ;
2016-12-28 04:25:52 +00:00
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |diidiidii " , kwlist , & fixedTimeStep , & numSolverIterations , & useSplitImpulse , & splitImpulsePenetrationThreshold , & numSubSteps ,
2017-03-01 21:48:57 +00:00
& collisionFilterMode , & contactBreakingThreshold , & maxNumCmdPer1ms , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
2016-12-01 06:24:20 +00:00
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-01 06:24:20 +00:00
{
2016-12-28 04:25:52 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
2016-12-01 06:24:20 +00:00
return NULL ;
}
2016-12-28 04:25:52 +00:00
2016-12-01 06:24:20 +00:00
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand ( sm ) ;
b3SharedMemoryStatusHandle statusHandle ;
if ( numSolverIterations > = 0 )
{
b3PhysicsParamSetNumSolverIterations ( command , numSolverIterations ) ;
}
2017-02-16 22:19:09 +00:00
2017-03-13 23:06:52 +00:00
if ( collisionFilterMode > = 0 )
2017-01-17 02:17:18 +00:00
{
b3PhysicsParamSetCollisionFilterMode ( command , collisionFilterMode ) ;
}
2016-12-05 19:54:56 +00:00
if ( numSubSteps > = 0 )
{
b3PhysicsParamSetNumSubSteps ( command , numSubSteps ) ;
}
2016-12-01 06:24:20 +00:00
if ( fixedTimeStep > = 0 )
{
b3PhysicsParamSetTimeStep ( command , fixedTimeStep ) ;
}
if ( useSplitImpulse > = 0 )
{
2017-03-13 23:06:52 +00:00
b3PhysicsParamSetUseSplitImpulse ( command , useSplitImpulse ) ;
2016-12-01 06:24:20 +00:00
}
if ( splitImpulsePenetrationThreshold > = 0 )
{
b3PhysicsParamSetSplitImpulsePenetrationThreshold ( command , splitImpulsePenetrationThreshold ) ;
}
2017-03-13 23:06:52 +00:00
if ( contactBreakingThreshold > = 0 )
2017-02-16 22:19:09 +00:00
{
2017-03-13 23:06:52 +00:00
b3PhysicsParamSetContactBreakingThreshold ( command , contactBreakingThreshold ) ;
2017-02-16 22:19:09 +00:00
}
2017-03-04 20:59:21 +00:00
//-1 is disables the maxNumCmdPer1ms feature, allow it
2017-03-13 23:06:52 +00:00
if ( maxNumCmdPer1ms > = - 1 )
2017-03-01 21:48:57 +00:00
{
2017-03-13 23:06:52 +00:00
b3PhysicsParamSetMaxNumCommandsPer1ms ( command , maxNumCmdPer1ms ) ;
2017-03-01 21:48:57 +00:00
}
2016-12-01 06:24:20 +00:00
//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 ;
}
2016-11-11 22:44:50 +00:00
// Load a robot from a URDF file (universal robot description format)
2016-06-10 22:07:41 +00:00
// function can be called without arguments and will default
// to position (0,0,1) with orientation(0,0,0,1)
2016-06-15 21:21:04 +00:00
// els(x,y,z) or
2016-06-10 22:07:41 +00:00
// loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w)
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_loadURDF ( PyObject * self , PyObject * args , PyObject * keywds )
2016-12-01 06:24:20 +00:00
{
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
2017-03-27 15:30:20 +00:00
int flags = 0 ;
2016-09-08 18:12:58 +00:00
2017-03-27 15:30:20 +00:00
static char * kwlist [ ] = { " fileName " , " basePosition " , " baseOrientation " , " useMaximalCoordinates " , " useFixedBase " , " flags " , " physicsClientId " , NULL } ;
2017-03-13 23:06:52 +00:00
static char * kwlistBackwardCompatible4 [ ] = { " fileName " , " startPosX " , " startPosY " , " startPosZ " , NULL } ;
static char * kwlistBackwardCompatible8 [ ] = { " fileName " , " startPosX " , " startPosY " , " startPosZ " , " startOrnX " , " startOrnY " , " startOrnZ " , " startOrnW " , NULL } ;
2016-12-28 04:25:52 +00:00
2017-03-13 23:06:52 +00:00
int bodyIndex = - 1 ;
const char * urdfFileName = " " ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
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 ;
2017-05-04 04:53:29 +00:00
int useMaximalCoordinates = - 1 ;
2017-03-13 23:06:52 +00:00
int useFixedBase = 0 ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
int backwardsCompatibilityArgs = 0 ;
b3PhysicsClientHandle sm = 0 ;
if ( PyArg_ParseTupleAndKeywords ( args , keywds , " sddd " , kwlistBackwardCompatible4 , & urdfFileName , & startPosX ,
& startPosY , & startPosZ ) )
2016-12-01 06:24:20 +00:00
{
2017-03-13 23:06:52 +00:00
backwardsCompatibilityArgs = 1 ;
2016-12-01 06:24:20 +00:00
}
2017-03-13 23:06:52 +00:00
else
2016-12-01 06:24:20 +00:00
{
2017-03-13 23:06:52 +00:00
PyErr_Clear ( ) ;
2016-12-01 06:24:20 +00:00
}
2017-03-13 23:06:52 +00:00
if ( PyArg_ParseTupleAndKeywords ( args , keywds , " sddddddd " , kwlistBackwardCompatible8 , & urdfFileName , & startPosX ,
& startPosY , & startPosZ , & startOrnX , & startOrnY , & startOrnZ , & startOrnW ) )
{
backwardsCompatibilityArgs = 1 ;
}
else
{
PyErr_Clear ( ) ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
if ( ! backwardsCompatibilityArgs )
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
PyObject * basePosObj = 0 ;
PyObject * baseOrnObj = 0 ;
double basePos [ 3 ] ;
double baseOrn [ 4 ] ;
2017-03-27 15:30:20 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " s|OOiiii " , kwlist , & urdfFileName , & basePosObj , & baseOrnObj , & useMaximalCoordinates , & useFixedBase , & flags , & physicsClientId ) )
2017-03-13 23:06:52 +00:00
{
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 ] ;
}
}
2016-12-28 04:25:52 +00:00
}
2017-03-13 23:06:52 +00:00
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
if ( strlen ( urdfFileName ) )
{
// printf("(%f, %f, %f) (%f, %f, %f, %f)\n",
// startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW);
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
b3SharedMemoryCommandHandle command =
b3LoadUrdfCommandInit ( sm , urdfFileName ) ;
2017-03-27 15:30:20 +00:00
b3LoadUrdfCommandSetFlags ( command , flags ) ;
2017-03-13 23:06:52 +00:00
// setting the initial position, orientation and other arguments are
// optional
b3LoadUrdfCommandSetStartPosition ( command , startPosX , startPosY , startPosZ ) ;
b3LoadUrdfCommandSetStartOrientation ( command , startOrnX , startOrnY ,
startOrnZ , startOrnW ) ;
2017-05-04 04:53:29 +00:00
if ( useMaximalCoordinates > = 0 )
2017-03-13 23:06:52 +00:00
{
2017-05-04 04:53:29 +00:00
b3LoadUrdfCommandSetUseMultiBody ( command , useMaximalCoordinates ) ;
2017-03-13 23:06:52 +00:00
}
if ( useFixedBase )
{
b3LoadUrdfCommandSetUseFixedBase ( command , 1 ) ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
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 ) ;
2016-06-13 17:11:28 +00:00
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_loadSDF ( PyObject * self , PyObject * args , PyObject * keywds )
{
const char * sdfFileName = " " ;
int numBodies = 0 ;
int i ;
int bodyIndicesOut [ MAX_SDF_BODIES ] ;
PyObject * pylist = 0 ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
b3SharedMemoryCommandHandle commandHandle ;
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
static char * kwlist [ ] = { " sdfFileName " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " s|i " , kwlist , & sdfFileName , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-12-29 05:51:54 +00:00
2017-03-13 23:06:52 +00:00
commandHandle = b3LoadSdfCommandInit ( sm , sdfFileName ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
if ( statusType ! = CMD_SDF_LOADING_COMPLETED )
2016-12-29 05:51:54 +00:00
{
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " Cannot load SDF file. " ) ;
2016-12-29 05:51:54 +00:00
return NULL ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
numBodies =
b3GetStatusBodyIndices ( statusHandle , bodyIndicesOut , MAX_SDF_BODIES ) ;
if ( numBodies > MAX_SDF_BODIES )
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " SDF exceeds body capacity " ) ;
2016-12-28 04:25:52 +00:00
return NULL ;
}
2017-03-13 23:06:52 +00:00
pylist = PyTuple_New ( numBodies ) ;
if ( numBodies > 0 & & numBodies < = MAX_SDF_BODIES )
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < numBodies ; i + + )
{
PyTuple_SetItem ( pylist , i , PyInt_FromLong ( bodyIndicesOut [ i ] ) ) ;
}
2016-12-28 04:25:52 +00:00
}
2017-03-13 23:06:52 +00:00
return pylist ;
2016-12-28 04:25:52 +00:00
}
2017-03-13 23:06:52 +00:00
// Reset the simulation to remove all loaded objects
static PyObject * pybullet_resetSimulation ( PyObject * self , PyObject * args , PyObject * keywds )
{
int physicsClientId = 0 ;
static char * kwlist [ ] = { " physicsClientId " , NULL } ;
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |i " , kwlist , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-07-18 06:50:11 +00:00
2017-03-13 23:06:52 +00:00
{
b3SharedMemoryStatusHandle statusHandle ;
statusHandle = b3SubmitClientCommandAndWaitStatus (
sm , b3InitResetSimulationCommand ( sm ) ) ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
//this method is obsolete, use pybullet_setJointMotorControl2 instead
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 ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm ;
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-10-23 14:14:50 +00:00
2017-03-13 23:06:52 +00:00
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 ;
2016-10-23 14:14:50 +00:00
2017-03-13 23:06:52 +00:00
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 ;
2016-12-28 04:25:52 +00:00
2017-03-13 23:06:52 +00:00
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 ;
}
2017-05-08 20:41:07 +00:00
static PyObject * pybullet_setJointMotorControlArray ( PyObject * self , PyObject * args , PyObject * keywds )
{
int bodyIndex , controlMode ;
PyObject * jointIndicesObj = 0 ;
PyObject * targetPositionsObj = 0 ;
PyObject * targetVelocitiesObj = 0 ;
PyObject * forcesObj = 0 ;
PyObject * kpsObj = 0 ;
PyObject * kdsObj = 0 ;
b3PhysicsClientHandle sm = 0 ;
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bodyIndex " , " jointIndices " , " controlMode " , " targetPositions " , " targetVelocities " , " forces " , " positionGains " , " velocityGains " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iOi|OOOOOi " , kwlist , & bodyIndex , & jointIndicesObj , & controlMode ,
& targetPositionsObj , & targetVelocitiesObj , & forcesObj , & kpsObj , & kdsObj , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
int numJoints ;
int i ;
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
struct b3JointInfo info ;
int numControlledDofs = 0 ;
PyObject * jointIndicesSeq = 0 ;
PyObject * targetVelocitiesSeq = 0 ;
PyObject * targetPositionsSeq = 0 ;
PyObject * forcesSeq = 0 ;
PyObject * kpsSeq = 0 ;
PyObject * kdsSeq = 0 ;
numJoints = b3GetNumJoints ( sm , bodyIndex ) ;
if ( ( controlMode ! = CONTROL_MODE_VELOCITY ) & &
( controlMode ! = CONTROL_MODE_TORQUE ) & &
( controlMode ! = CONTROL_MODE_POSITION_VELOCITY_PD ) )
{
PyErr_SetString ( SpamError , " Illegral control mode. " ) ;
return NULL ;
}
jointIndicesSeq = PySequence_Fast ( jointIndicesObj , " expected a sequence of joint indices " ) ;
if ( jointIndicesSeq = = 0 )
{
PyErr_SetString ( SpamError , " expected a sequence of joint indices " ) ;
return NULL ;
}
numControlledDofs = PySequence_Size ( jointIndicesObj ) ;
if ( numControlledDofs = = 0 )
{
Py_DECREF ( jointIndicesSeq ) ;
Py_INCREF ( Py_None ) ;
return Py_None ;
}
{
int i ;
for ( i = 0 ; i < numControlledDofs ; i + + )
{
int jointIndex = pybullet_internalGetFloatFromSequence ( jointIndicesSeq , i ) ;
if ( ( jointIndex > = numJoints ) | | ( jointIndex < 0 ) )
{
Py_DECREF ( jointIndicesSeq ) ;
PyErr_SetString ( SpamError , " Joint index out-of-range. " ) ;
return NULL ;
}
}
}
if ( targetVelocitiesObj )
{
int num = PySequence_Size ( targetVelocitiesObj ) ;
if ( num ! = numControlledDofs )
{
Py_DECREF ( jointIndicesSeq ) ;
PyErr_SetString ( SpamError , " number of target velocies should match the number of joint indices " ) ;
return NULL ;
}
targetVelocitiesSeq = PySequence_Fast ( targetVelocitiesObj , " expected a sequence of target velocities " ) ;
}
if ( targetPositionsObj )
{
int num = PySequence_Size ( targetPositionsObj ) ;
if ( num ! = numControlledDofs )
{
Py_DECREF ( jointIndicesSeq ) ;
if ( targetVelocitiesSeq )
{
Py_DECREF ( targetVelocitiesSeq ) ;
}
PyErr_SetString ( SpamError , " number of target positions should match the number of joint indices " ) ;
return NULL ;
}
targetPositionsSeq = PySequence_Fast ( targetPositionsObj , " expected a sequence of target positions " ) ;
}
if ( forcesObj )
{
int num = PySequence_Size ( forcesObj ) ;
if ( num ! = numControlledDofs )
{
Py_DECREF ( jointIndicesSeq ) ;
if ( targetVelocitiesSeq )
{
Py_DECREF ( targetVelocitiesSeq ) ;
}
if ( targetPositionsSeq )
{
Py_DECREF ( targetPositionsSeq ) ;
}
PyErr_SetString ( SpamError , " number of forces should match the joint indices " ) ;
return NULL ;
}
forcesSeq = PySequence_Fast ( forcesObj , " expected a sequence of forces " ) ;
}
if ( kpsObj )
{
int num = PySequence_Size ( kpsObj ) ;
if ( num ! = numControlledDofs )
{
Py_DECREF ( jointIndicesSeq ) ;
if ( targetVelocitiesSeq )
{
Py_DECREF ( targetVelocitiesSeq ) ;
}
if ( targetPositionsSeq )
{
Py_DECREF ( targetPositionsSeq ) ;
}
if ( forcesSeq )
{
Py_DECREF ( forcesSeq ) ;
}
PyErr_SetString ( SpamError , " number of kps should match the joint indices " ) ;
return NULL ;
}
kpsSeq = PySequence_Fast ( kpsObj , " expected a sequence of kps " ) ;
}
if ( kdsObj )
{
int num = PySequence_Size ( kdsObj ) ;
if ( num ! = numControlledDofs )
{
Py_DECREF ( jointIndicesSeq ) ;
if ( targetVelocitiesSeq )
{
Py_DECREF ( targetVelocitiesSeq ) ;
}
if ( targetPositionsSeq )
{
Py_DECREF ( targetPositionsSeq ) ;
}
if ( forcesSeq )
{
Py_DECREF ( forcesSeq ) ;
}
if ( kpsSeq )
{
Py_DECREF ( kpsSeq ) ;
}
PyErr_SetString ( SpamError , " number of kds should match the number of joint indices " ) ;
return NULL ;
}
kdsSeq = PySequence_Fast ( kdsObj , " expected a sequence of kds " ) ;
}
commandHandle = b3JointControlCommandInit2 ( sm , bodyIndex , controlMode ) ;
for ( i = 0 ; i < numControlledDofs ; i + + )
{
double targetVelocity = 0.0 ;
if ( targetVelocitiesSeq )
{
targetVelocity = pybullet_internalGetFloatFromSequence ( targetVelocitiesSeq , i ) ;
}
double targetPosition = 0.0 ;
if ( targetPositionsSeq )
{
targetPosition = pybullet_internalGetFloatFromSequence ( targetPositionsSeq , i ) ;
}
double force = 100000.0 ;
if ( forcesSeq )
{
force = pybullet_internalGetFloatFromSequence ( forcesSeq , i ) ;
}
double kp = 0.1 ;
if ( kpsSeq )
{
kp = pybullet_internalGetFloatFromSequence ( kpsSeq , i ) ;
}
double kd = 1.0 ;
if ( kdsSeq )
{
kd = pybullet_internalGetFloatFromSequence ( kdsSeq , i ) ;
}
int jointIndex = pybullet_internalGetFloatFromSequence ( jointIndicesSeq , i ) ;
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 , force ) ;
break ;
}
case CONTROL_MODE_TORQUE :
{
b3JointControlSetDesiredForceTorque ( commandHandle , info . m_uIndex ,
force ) ;
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 , force ) ;
break ;
}
default :
{
}
} ;
}
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
Py_DECREF ( jointIndicesSeq ) ;
Py_INCREF ( Py_None ) ;
return Py_None ;
}
// PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
// return NULL;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_setJointMotorControl2 ( PyObject * self , PyObject * args , PyObject * keywds )
{
int bodyIndex , jointIndex , controlMode ;
double targetPosition = 0.0 ;
double targetVelocity = 0.0 ;
double force = 100000.0 ;
double kp = 0.1 ;
double kd = 1.0 ;
b3PhysicsClientHandle sm = 0 ;
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bodyIndex " , " jointIndex " , " controlMode " , " targetPosition " , " targetVelocity " , " force " , " positionGain " , " velocityGain " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iii|dddddi " , kwlist , & bodyIndex , & jointIndex , & controlMode ,
& targetPosition , & targetVelocity , & force , & kp , & kd , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-10-23 14:14:50 +00:00
2017-03-13 23:06:52 +00:00
{
int numJoints ;
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
struct b3JointInfo info ;
2016-10-23 14:14:50 +00:00
2017-03-13 23:06:52 +00:00
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 ;
}
2016-10-23 14:14:50 +00:00
2017-03-13 23:06:52 +00:00
commandHandle = b3JointControlCommandInit2 ( sm , bodyIndex , controlMode ) ;
2016-10-23 14:14:50 +00:00
2017-03-13 23:06:52 +00:00
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 , force ) ;
break ;
}
case CONTROL_MODE_TORQUE :
{
b3JointControlSetDesiredForceTorque ( commandHandle , info . m_uIndex ,
force ) ;
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 , force ) ;
break ;
}
default :
{
}
} ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
Py_INCREF ( Py_None ) ;
return Py_None ;
}
// PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
// return NULL;
2016-10-23 14:14:50 +00:00
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_setRealTimeSimulation ( PyObject * self ,
PyObject * args ,
PyObject * keywds )
{
int enableRealTimeSimulation = 0 ;
int ret ;
b3PhysicsClientHandle sm = 0 ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " enableRealTimeSimulation " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & enableRealTimeSimulation , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-05-24 22:29:26 +00:00
2017-03-13 23:06:52 +00:00
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand ( sm ) ;
b3SharedMemoryStatusHandle statusHandle ;
ret =
b3PhysicsParamSetRealTimeSimulation ( command , enableRealTimeSimulation ) ;
2016-05-10 07:57:54 +00:00
2017-03-13 23:06:52 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
// ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-05-24 22:29:26 +00:00
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_setInternalSimFlags ( PyObject * self ,
PyObject * args , PyObject * keywds )
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
int flags = 0 ;
2016-12-28 04:25:52 +00:00
int ret ;
int physicsClientId = 0 ;
2016-12-29 05:51:54 +00:00
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " flags " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & flags , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand ( sm ) ;
b3SharedMemoryStatusHandle statusHandle ;
2016-09-08 22:22:41 +00:00
2017-03-13 23:06:52 +00:00
ret =
b3PhysicsParamSetInternalSimFlags ( command , flags ) ;
2016-05-24 22:29:26 +00:00
2017-03-13 23:06:52 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
// ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
2016-09-08 18:12:58 +00:00
2016-12-28 04:25:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-03-13 23:06:52 +00:00
// Set the gravity of the world with (x, y, z) arguments
static PyObject * pybullet_setGravity ( PyObject * self , PyObject * args , PyObject * keywds )
{
{
double gravX = 0.0 ;
double gravY = 0.0 ;
double gravZ = - 10.0 ;
int ret ;
b3PhysicsClientHandle sm = 0 ;
b3SharedMemoryCommandHandle command ;
b3SharedMemoryStatusHandle statusHandle ;
int physicsClientId = 0 ;
static char * kwlist [ ] = { " gravX " , " gravY " , " gravZ " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ddd|i " , kwlist , & gravX , & gravY , & gravZ , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
command = b3InitPhysicsParamCommand ( sm ) ;
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 ;
2016-06-27 01:18:30 +00:00
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_setTimeStep ( PyObject * self , PyObject * args , PyObject * keywds )
{
double timeStep = 0.001 ;
int ret ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
2016-12-29 05:51:54 +00:00
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " timeStep " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " d|i " , kwlist , & timeStep , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand ( sm ) ;
2017-03-13 23:06:52 +00:00
b3SharedMemoryStatusHandle statusHandle ;
2016-09-08 22:15:58 +00:00
2017-03-13 23:06:52 +00:00
ret = b3PhysicsParamSetTimeStep ( command , timeStep ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
Py_INCREF ( Py_None ) ;
return Py_None ;
}
}
static PyObject *
pybullet_setDefaultContactERP ( PyObject * self , PyObject * args , PyObject * keywds )
{
double defaultContactERP = 0.005 ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " defaultContactERP " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " d|i " , kwlist , & defaultContactERP , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
int ret ;
b3SharedMemoryStatusHandle statusHandle ;
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand ( sm ) ;
ret = b3PhysicsParamSetDefaultContactERP ( command , defaultContactERP ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-09-08 22:15:58 +00:00
}
2016-11-28 23:11:34 +00:00
static int pybullet_internalGetBaseVelocity (
2017-03-13 23:06:52 +00:00
int bodyIndex , double baseLinearVelocity [ 3 ] , double baseAngularVelocity [ 3 ] , b3PhysicsClientHandle sm )
{
2016-11-28 23:11:34 +00:00
baseLinearVelocity [ 0 ] = 0. ;
baseLinearVelocity [ 1 ] = 0. ;
baseLinearVelocity [ 2 ] = 0. ;
baseAngularVelocity [ 0 ] = 0. ;
baseAngularVelocity [ 1 ] = 0. ;
baseAngularVelocity [ 2 ] = 0. ;
2017-03-13 23:06:52 +00:00
if ( 0 = = sm )
{
2016-11-28 23:11:34 +00:00
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[];
2017-03-13 23:06:52 +00:00
if ( status_type ! = CMD_ACTUAL_STATE_UPDATE_COMPLETED )
{
2016-11-28 23:11:34 +00:00
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 ;
}
2016-05-24 22:29:26 +00:00
2016-06-10 22:07:41 +00:00
// Internal function used to get the base position and orientation
// Orientation is returned in quaternions
2016-09-08 18:12:58 +00:00
static int pybullet_internalGetBasePositionAndOrientation (
2017-03-13 23:06:52 +00:00
int bodyIndex , double basePosition [ 3 ] , double baseOrientation [ 4 ] , b3PhysicsClientHandle sm )
{
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 ;
2016-05-24 22:29:26 +00:00
}
2016-05-10 07:57:54 +00:00
2016-06-10 22:07:41 +00:00
// 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)
2016-09-08 18:12:58 +00:00
static PyObject * pybullet_getBasePositionAndOrientation ( PyObject * self ,
2017-03-13 23:06:52 +00:00
PyObject * args , PyObject * keywds )
{
int bodyUniqueId = - 1 ;
double basePosition [ 3 ] ;
double baseOrientation [ 4 ] ;
PyObject * pylistPos ;
PyObject * pylistOrientation ;
b3PhysicsClientHandle sm = 0 ;
2016-06-13 17:11:28 +00:00
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bodyUniqueId " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & bodyUniqueId , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-04-30 18:18:54 +00:00
2017-03-13 23:06:52 +00:00
if ( 0 = = pybullet_internalGetBasePositionAndOrientation (
bodyUniqueId , basePosition , baseOrientation , sm ) )
{
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 ;
}
}
2016-11-28 23:11:34 +00:00
static PyObject * pybullet_getBaseVelocity ( PyObject * self ,
2017-03-13 23:06:52 +00:00
PyObject * args , PyObject * keywds )
{
2016-12-28 04:25:52 +00:00
int bodyUniqueId = - 1 ;
2016-11-28 23:11:34 +00:00
double baseLinearVelocity [ 3 ] ;
double baseAngularVelocity [ 3 ] ;
2017-03-13 23:06:52 +00:00
PyObject * pylistLinVel = 0 ;
PyObject * pylistAngVel = 0 ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2016-11-28 23:11:34 +00:00
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " bodyUniqueId " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & bodyUniqueId , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
2016-11-28 23:11:34 +00:00
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
2016-11-28 23:11:34 +00:00
return NULL ;
}
2017-03-13 23:06:52 +00:00
2016-11-28 23:11:34 +00:00
if ( 0 = = pybullet_internalGetBaseVelocity (
2017-03-13 23:06:52 +00:00
bodyUniqueId , baseLinearVelocity , baseAngularVelocity , sm ) )
{
2016-11-28 23:11:34 +00:00
PyErr_SetString ( SpamError ,
2017-03-13 23:06:52 +00:00
" getBaseVelocity failed. " ) ;
2016-11-28 23:11:34 +00:00
return NULL ;
}
{
PyObject * item ;
int i ;
int num = 3 ;
pylistLinVel = PyTuple_New ( num ) ;
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < num ; i + + )
{
2016-11-28 23:11:34 +00:00
item = PyFloat_FromDouble ( baseLinearVelocity [ i ] ) ;
PyTuple_SetItem ( pylistLinVel , i , item ) ;
}
}
{
PyObject * item ;
int i ;
int num = 3 ;
pylistAngVel = PyTuple_New ( num ) ;
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < num ; i + + )
{
2016-11-28 23:11:34 +00:00
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 ;
}
}
2016-12-28 04:25:52 +00:00
static PyObject * pybullet_getNumBodies ( PyObject * self , PyObject * args , PyObject * keywds )
2016-09-27 19:13:45 +00:00
{
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |i " , kwlist , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-09-27 19:13:45 +00:00
2017-03-13 23:06:52 +00:00
{
int numBodies = b3GetNumBodies ( sm ) ;
2016-09-27 19:13:45 +00:00
# if PY_MAJOR_VERSION >= 3
2017-03-13 23:06:52 +00:00
return PyLong_FromLong ( numBodies ) ;
2016-09-27 19:13:45 +00:00
# else
2017-03-13 23:06:52 +00:00
return PyInt_FromLong ( numBodies ) ;
2016-09-27 19:13:45 +00:00
# endif
2017-03-13 23:06:52 +00:00
}
2016-09-27 19:13:45 +00:00
}
2016-12-28 04:25:52 +00:00
static PyObject * pybullet_getBodyUniqueId ( PyObject * self , PyObject * args , PyObject * keywds )
2016-09-27 19:13:45 +00:00
{
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
int serialIndex = - 1 ;
b3PhysicsClientHandle sm = 0 ;
2016-12-28 04:25:52 +00:00
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " serialIndex " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & serialIndex , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-09-27 19:13:45 +00:00
{
2017-03-13 23:06:52 +00:00
int bodyUniqueId = - 1 ;
bodyUniqueId = b3GetBodyUniqueId ( sm , serialIndex ) ;
2016-09-27 19:13:45 +00:00
# if PY_MAJOR_VERSION >= 3
2017-03-13 23:06:52 +00:00
return PyLong_FromLong ( bodyUniqueId ) ;
2016-09-27 19:13:45 +00:00
# else
2017-03-13 23:06:52 +00:00
return PyInt_FromLong ( bodyUniqueId ) ;
2016-09-27 19:13:45 +00:00
# endif
2017-03-13 23:06:52 +00:00
}
2016-09-27 19:13:45 +00:00
}
2017-05-04 04:53:29 +00:00
static PyObject * pybullet_removeBody ( PyObject * self , PyObject * args , PyObject * keywds )
{
{
int bodyUniqueId = - 1 ;
b3PhysicsClientHandle sm = 0 ;
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bodyUniqueId " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & bodyUniqueId , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
if ( bodyUniqueId > = 0 )
{
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
if ( b3CanSubmitCommand ( sm ) )
{
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , b3InitRemoveBodyCommand ( sm , bodyUniqueId ) ) ;
statusType = b3GetStatusType ( statusHandle ) ;
}
}
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2016-12-28 04:25:52 +00:00
static PyObject * pybullet_getBodyInfo ( PyObject * self , PyObject * args , PyObject * keywds )
2016-09-27 19:13:45 +00:00
{
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
int bodyUniqueId = - 1 ;
b3PhysicsClientHandle sm = 0 ;
2016-12-28 04:25:52 +00:00
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bodyUniqueId " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & bodyUniqueId , & physicsClientId ) )
2016-09-27 19:13:45 +00:00
{
2017-03-13 23:06:52 +00:00
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
2016-12-28 04:25:52 +00:00
return NULL ;
2016-09-27 19:13:45 +00:00
}
2017-03-13 23:06:52 +00:00
{
struct b3BodyInfo info ;
if ( b3GetBodyInfo ( sm , bodyUniqueId , & info ) )
{
2017-03-29 21:56:05 +00:00
PyObject * pyListJointInfo = PyTuple_New ( 2 ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( pyListJointInfo , 0 , PyString_FromString ( info . m_baseName ) ) ;
2017-03-29 22:37:33 +00:00
PyTuple_SetItem ( pyListJointInfo , 1 , PyString_FromString ( info . m_bodyName ) ) ;
2017-03-13 23:06:52 +00:00
return pyListJointInfo ;
}
}
2016-12-28 04:25:52 +00:00
}
2017-03-14 20:13:16 +00:00
PyErr_SetString ( SpamError , " Couldn't get body info " ) ;
2017-03-13 23:06:52 +00:00
return NULL ;
2016-09-27 19:13:45 +00:00
}
2017-01-23 03:08:31 +00:00
static PyObject * pybullet_getConstraintInfo ( PyObject * self , PyObject * args , PyObject * keywds )
{
2017-03-13 23:06:52 +00:00
{
int constraintUniqueId = - 1 ;
b3PhysicsClientHandle sm = 0 ;
int physicsClientId = 0 ;
static char * kwlist [ ] = { " constraintUniqueId " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & constraintUniqueId , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
struct b3UserConstraint constraintInfo ;
if ( b3GetUserConstraintInfo ( sm , constraintUniqueId , & constraintInfo ) )
{
PyObject * pyListConstraintInfo = PyTuple_New ( 11 ) ;
PyTuple_SetItem ( pyListConstraintInfo , 0 , PyLong_FromLong ( constraintInfo . m_parentBodyIndex ) ) ;
PyTuple_SetItem ( pyListConstraintInfo , 1 , PyLong_FromLong ( constraintInfo . m_parentJointIndex ) ) ;
PyTuple_SetItem ( pyListConstraintInfo , 2 , PyLong_FromLong ( constraintInfo . m_childBodyIndex ) ) ;
PyTuple_SetItem ( pyListConstraintInfo , 3 , PyLong_FromLong ( constraintInfo . m_childJointIndex ) ) ;
PyTuple_SetItem ( pyListConstraintInfo , 4 , PyLong_FromLong ( constraintInfo . m_jointType ) ) ;
2017-01-23 03:08:31 +00:00
{
2017-03-13 23:06:52 +00:00
PyObject * axisObj = PyTuple_New ( 3 ) ;
PyTuple_SetItem ( axisObj , 0 , PyFloat_FromDouble ( constraintInfo . m_jointAxis [ 0 ] ) ) ;
PyTuple_SetItem ( axisObj , 1 , PyFloat_FromDouble ( constraintInfo . m_jointAxis [ 1 ] ) ) ;
PyTuple_SetItem ( axisObj , 2 , PyFloat_FromDouble ( constraintInfo . m_jointAxis [ 2 ] ) ) ;
PyTuple_SetItem ( pyListConstraintInfo , 5 , axisObj ) ;
2017-01-23 03:08:31 +00:00
}
2017-03-13 23:06:52 +00:00
{
PyObject * parentFramePositionObj = PyTuple_New ( 3 ) ;
PyTuple_SetItem ( parentFramePositionObj , 0 , PyFloat_FromDouble ( constraintInfo . m_parentFrame [ 0 ] ) ) ;
PyTuple_SetItem ( parentFramePositionObj , 1 , PyFloat_FromDouble ( constraintInfo . m_parentFrame [ 1 ] ) ) ;
PyTuple_SetItem ( parentFramePositionObj , 2 , PyFloat_FromDouble ( constraintInfo . m_parentFrame [ 2 ] ) ) ;
PyTuple_SetItem ( pyListConstraintInfo , 6 , parentFramePositionObj ) ;
2017-01-23 03:08:31 +00:00
}
2017-03-13 23:06:52 +00:00
{
PyObject * childFramePositionObj = PyTuple_New ( 3 ) ;
PyTuple_SetItem ( childFramePositionObj , 0 , PyFloat_FromDouble ( constraintInfo . m_childFrame [ 0 ] ) ) ;
PyTuple_SetItem ( childFramePositionObj , 1 , PyFloat_FromDouble ( constraintInfo . m_childFrame [ 1 ] ) ) ;
PyTuple_SetItem ( childFramePositionObj , 2 , PyFloat_FromDouble ( constraintInfo . m_childFrame [ 2 ] ) ) ;
PyTuple_SetItem ( pyListConstraintInfo , 7 , childFramePositionObj ) ;
}
{
PyObject * parentFrameOrientationObj = PyTuple_New ( 4 ) ;
PyTuple_SetItem ( parentFrameOrientationObj , 0 , PyFloat_FromDouble ( constraintInfo . m_parentFrame [ 3 ] ) ) ;
PyTuple_SetItem ( parentFrameOrientationObj , 1 , PyFloat_FromDouble ( constraintInfo . m_parentFrame [ 4 ] ) ) ;
PyTuple_SetItem ( parentFrameOrientationObj , 2 , PyFloat_FromDouble ( constraintInfo . m_parentFrame [ 5 ] ) ) ;
PyTuple_SetItem ( parentFrameOrientationObj , 3 , PyFloat_FromDouble ( constraintInfo . m_parentFrame [ 6 ] ) ) ;
PyTuple_SetItem ( pyListConstraintInfo , 8 , parentFrameOrientationObj ) ;
}
{
PyObject * childFrameOrientation = PyTuple_New ( 4 ) ;
PyTuple_SetItem ( childFrameOrientation , 0 , PyFloat_FromDouble ( constraintInfo . m_childFrame [ 3 ] ) ) ;
PyTuple_SetItem ( childFrameOrientation , 1 , PyFloat_FromDouble ( constraintInfo . m_childFrame [ 4 ] ) ) ;
PyTuple_SetItem ( childFrameOrientation , 2 , PyFloat_FromDouble ( constraintInfo . m_childFrame [ 5 ] ) ) ;
PyTuple_SetItem ( childFrameOrientation , 3 , PyFloat_FromDouble ( constraintInfo . m_childFrame [ 6 ] ) ) ;
PyTuple_SetItem ( pyListConstraintInfo , 9 , childFrameOrientation ) ;
}
PyTuple_SetItem ( pyListConstraintInfo , 10 , PyFloat_FromDouble ( constraintInfo . m_maxAppliedForce ) ) ;
2017-01-23 03:08:31 +00:00
2017-03-13 23:06:52 +00:00
return pyListConstraintInfo ;
}
}
}
2017-01-23 03:08:31 +00:00
2017-03-14 20:13:16 +00:00
PyErr_SetString ( SpamError , " Couldn't get user constraint info " ) ;
2017-03-13 23:06:52 +00:00
return NULL ;
2017-01-23 03:08:31 +00:00
}
2017-05-04 01:25:25 +00:00
static PyObject * pybullet_getConstraintUniqueId ( PyObject * self , PyObject * args , PyObject * keywds )
{
int physicsClientId = 0 ;
int serialIndex = - 1 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " serialIndex " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & serialIndex , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
int userConstraintId = - 1 ;
userConstraintId = b3GetUserConstraintId ( sm , serialIndex ) ;
# if PY_MAJOR_VERSION >= 3
return PyLong_FromLong ( userConstraintId ) ;
# else
return PyInt_FromLong ( userConstraintId ) ;
# endif
}
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getNumConstraints ( PyObject * self , PyObject * args , PyObject * keywds )
2016-09-27 19:13:45 +00:00
{
2017-03-13 23:06:52 +00:00
int numConstraints = 0 ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |i " , kwlist , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
numConstraints = b3GetNumUserConstraints ( sm ) ;
2016-05-10 07:57:54 +00:00
# if PY_MAJOR_VERSION >= 3
2017-03-13 23:06:52 +00:00
return PyLong_FromLong ( numConstraints ) ;
2016-05-10 07:57:54 +00:00
# else
2017-03-13 23:06:52 +00:00
return PyInt_FromLong ( numConstraints ) ;
2016-05-10 07:57:54 +00:00
# endif
}
2017-03-13 23:06:52 +00:00
// 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 , PyObject * keywds )
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
int bodyUniqueId = - 1 ;
int numJoints = 0 ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
2017-03-13 23:06:52 +00:00
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " bodyUniqueId " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & bodyUniqueId , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-06-23 06:21:47 +00:00
2017-03-13 23:06:52 +00:00
numJoints = b3GetNumJoints ( sm , bodyUniqueId ) ;
# if PY_MAJOR_VERSION >= 3
return PyLong_FromLong ( numJoints ) ;
# else
return PyInt_FromLong ( numJoints ) ;
# endif
}
2016-06-14 21:58:36 +00:00
2017-03-13 23:06:52 +00:00
// Initalize all joint positions given a list of values
static PyObject * pybullet_resetJointState ( PyObject * self , PyObject * args , PyObject * keywds )
{
{
int bodyUniqueId ;
int jointIndex ;
double targetValue ;
2017-03-23 17:29:16 +00:00
double targetVelocity = 0 ;
2017-03-13 23:06:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
2017-03-23 17:29:16 +00:00
static char * kwlist [ ] = { " bodyUniqueId " , " jointIndex " , " targetValue " , " targetVelocity " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iid|di " , kwlist , & bodyUniqueId , & jointIndex , & targetValue , & targetVelocity , & physicsClientId ) )
2017-03-13 23:06:52 +00:00
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
{
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int numJoints ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
numJoints = b3GetNumJoints ( sm , bodyUniqueId ) ;
if ( ( jointIndex > = numJoints ) | | ( jointIndex < 0 ) )
{
PyErr_SetString ( SpamError , " Joint index out-of-range. " ) ;
return NULL ;
}
2016-06-15 21:21:04 +00:00
2017-03-13 23:06:52 +00:00
commandHandle = b3CreatePoseCommandInit ( sm , bodyUniqueId ) ;
2016-11-28 23:11:34 +00:00
2017-03-13 23:06:52 +00:00
b3CreatePoseCommandSetJointPosition ( sm , commandHandle , jointIndex ,
targetValue ) ;
2016-11-28 23:11:34 +00:00
2017-03-23 17:29:16 +00:00
b3CreatePoseCommandSetJointVelocity ( sm , commandHandle , jointIndex ,
targetVelocity ) ;
2017-03-13 23:06:52 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
}
}
2017-03-14 20:13:16 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
2017-03-13 23:06:52 +00:00
}
2016-11-28 23:11:34 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_resetBaseVelocity ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-28 23:11:34 +00:00
{
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " objectUniqueId " , " linearVelocity " , " angularVelocity " , " physicsClientId " , NULL } ;
2016-11-28 23:11:34 +00:00
{
2017-03-13 23:06:52 +00:00
int bodyIndex = 0 ;
PyObject * linVelObj = 0 ;
PyObject * angVelObj = 0 ;
double linVel [ 3 ] = { 0 , 0 , 0 } ;
double angVel [ 3 ] = { 0 , 0 , 0 } ;
int physicsClientId = 0 ;
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|OOi " , kwlist , & bodyIndex , & linVelObj , & angVelObj , & physicsClientId ) )
2016-11-28 23:11:34 +00:00
{
return NULL ;
}
2016-12-28 04:25:52 +00:00
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-12-28 04:25:52 +00:00
2016-11-28 23:11:34 +00:00
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 ;
}
2016-09-08 18:12:58 +00:00
// 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 ,
2017-03-13 23:06:52 +00:00
PyObject * args , PyObject * keywds )
{
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
int bodyUniqueId ;
PyObject * posObj ;
PyObject * ornObj ;
double pos [ 3 ] ;
double orn [ 4 ] ; // as a quaternion
b3PhysicsClientHandle sm = 0 ;
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bodyUniqueId " , " posObj " , " ornObj " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iOO|i " , kwlist , & bodyUniqueId , & posObj , & ornObj , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
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 , bodyUniqueId ) ;
b3CreatePoseCommandSetBasePosition ( commandHandle , pos [ 0 ] , pos [ 1 ] , pos [ 2 ] ) ;
b3CreatePoseCommandSetBaseOrientation ( commandHandle , orn [ 0 ] , orn [ 1 ] ,
orn [ 2 ] , orn [ 3 ] ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
}
2016-12-28 04:25:52 +00:00
}
2017-03-14 20:13:16 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-06-14 21:08:24 +00:00
}
2016-06-27 01:18:30 +00:00
2016-06-14 21:58:36 +00:00
// Get the a single joint info for a specific bodyIndex
//
2016-06-15 21:21:04 +00:00
// Args:
// bodyIndex - integer indicating body in simulation
// jointIndex - integer indicating joint for a specific body
//
2016-06-14 21:58:36 +00:00
// Joint information includes:
// index, name, type, q-index, u-index,
// flags, joint damping, joint friction
//
2016-09-08 18:12:58 +00:00
// The format of the returned list is
2016-06-14 21:58:36 +00:00
// [int, str, int, int, int, int, float, float]
//
2016-06-14 21:08:24 +00:00
// TODO(hellojas): get joint positions for a body
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getJointInfo ( PyObject * self , PyObject * args , PyObject * keywds )
{
PyObject * pyListJointInfo ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
struct b3JointInfo info ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
int bodyUniqueId = - 1 ;
int jointIndex = - 1 ;
2017-04-25 21:58:30 +00:00
int jointInfoSize = 13 ; // size of struct b3JointInfo
2017-03-13 23:06:52 +00:00
b3PhysicsClientHandle sm = 0 ;
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bodyUniqueId " , " jointIndex " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ii|i " , kwlist , & bodyUniqueId , & jointIndex , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
{
{
// printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex);
pyListJointInfo = PyTuple_New ( jointInfoSize ) ;
if ( b3GetJointInfo ( sm , bodyUniqueId , 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 ) ) ;
2017-03-26 20:06:46 +00:00
PyTuple_SetItem ( pyListJointInfo , 8 ,
PyFloat_FromDouble ( info . m_jointLowerLimit ) ) ;
PyTuple_SetItem ( pyListJointInfo , 9 ,
PyFloat_FromDouble ( info . m_jointUpperLimit ) ) ;
PyTuple_SetItem ( pyListJointInfo , 10 ,
PyFloat_FromDouble ( info . m_jointMaxForce ) ) ;
PyTuple_SetItem ( pyListJointInfo , 11 ,
PyFloat_FromDouble ( info . m_jointMaxVelocity ) ) ;
2017-04-25 21:58:30 +00:00
PyTuple_SetItem ( pyListJointInfo , 12 ,
PyString_FromString ( info . m_linkName ) ) ;
2017-03-26 20:06:46 +00:00
2017-03-13 23:06:52 +00:00
return pyListJointInfo ;
}
else
{
PyErr_SetString ( SpamError , " GetJointInfo failed. " ) ;
return NULL ;
}
}
}
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-09-08 18:12:58 +00:00
}
2016-06-14 21:08:24 +00:00
// Returns the state of a specific joint in a given bodyIndex
2016-06-15 21:21:04 +00:00
//
// Args:
// bodyIndex - integer indicating body in simulation
// jointIndex - integer indicating joint for a specific body
//
2016-06-14 21:08:24 +00:00
// 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]
2016-09-08 18:12:58 +00:00
// TODO(hellojas): check accuracy of position and velocity
2016-06-14 21:08:24 +00:00
// TODO(hellojas): check force torque values
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getJointState ( PyObject * self , PyObject * args , PyObject * keywds )
{
PyObject * pyListJointForceTorque ;
PyObject * pyListJointState ;
2016-09-27 19:13:45 +00:00
2017-03-13 23:06:52 +00:00
struct b3JointSensorState sensorState ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
int bodyUniqueId = - 1 ;
int jointIndex = - 1 ;
int sensorStateSize = 4 ; // size of struct b3JointSensorState
int forceTorqueSize = 6 ; // size of force torque list from b3JointSensorState
int j ;
2016-06-14 21:08:24 +00:00
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
static char * kwlist [ ] = { " bodyUniqueId " , " jointIndex " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ii|i " , kwlist , & bodyUniqueId , & jointIndex , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-06-24 14:31:17 +00:00
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
{
int status_type = 0 ;
b3SharedMemoryCommandHandle cmd_handle ;
b3SharedMemoryStatusHandle status_handle ;
2016-09-24 18:25:05 +00:00
2017-03-13 23:06:52 +00:00
if ( bodyUniqueId < 0 )
{
PyErr_SetString ( SpamError , " getJointState failed; invalid bodyIndex " ) ;
return NULL ;
}
if ( jointIndex < 0 )
{
PyErr_SetString ( SpamError , " getJointState failed; invalid jointIndex " ) ;
return NULL ;
}
2016-09-23 22:57:35 +00:00
2017-03-13 23:06:52 +00:00
cmd_handle =
b3RequestActualStateCommandInit ( sm , bodyUniqueId ) ;
status_handle =
b3SubmitClientCommandAndWaitStatus ( sm , cmd_handle ) ;
2016-06-14 21:08:24 +00:00
2017-03-13 23:06:52 +00:00
status_type = b3GetStatusType ( status_handle ) ;
if ( status_type ! = CMD_ACTUAL_STATE_UPDATE_COMPLETED )
{
PyErr_SetString ( SpamError , " getJointState failed. " ) ;
return NULL ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
pyListJointState = PyTuple_New ( sensorStateSize ) ;
pyListJointForceTorque = PyTuple_New ( forceTorqueSize ) ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
if ( b3GetJointState ( sm , status_handle , jointIndex , & sensorState ) )
{
PyTuple_SetItem ( pyListJointState , 0 ,
PyFloat_FromDouble ( sensorState . m_jointPosition ) ) ;
PyTuple_SetItem ( pyListJointState , 1 ,
PyFloat_FromDouble ( sensorState . m_jointVelocity ) ) ;
2017-01-21 02:13:24 +00:00
2017-03-13 23:06:52 +00:00
for ( j = 0 ; j < forceTorqueSize ; j + + )
{
PyTuple_SetItem ( pyListJointForceTorque , j ,
PyFloat_FromDouble ( sensorState . m_jointForceTorque [ j ] ) ) ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( pyListJointState , 2 , pyListJointForceTorque ) ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( pyListJointState , 3 ,
PyFloat_FromDouble ( sensorState . m_jointMotorTorque ) ) ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
return pyListJointState ;
}
else
{
PyErr_SetString ( SpamError , " getJointState failed (2). " ) ;
return NULL ;
}
}
}
2016-06-14 21:08:24 +00:00
2017-03-13 23:06:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-09-08 18:12:58 +00:00
}
2016-06-17 01:46:34 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getLinkState ( PyObject * self , PyObject * args , PyObject * keywds )
{
PyObject * pyLinkState ;
PyObject * pyLinkStateWorldPosition ;
PyObject * pyLinkStateWorldOrientation ;
PyObject * pyLinkStateLocalInertialPosition ;
PyObject * pyLinkStateLocalInertialOrientation ;
PyObject * pyLinkStateWorldLinkFramePosition ;
PyObject * pyLinkStateWorldLinkFrameOrientation ;
2017-03-26 20:06:46 +00:00
PyObject * pyLinkStateWorldLinkLinearVelocity ;
PyObject * pyLinkStateWorldLinkAngularVelocity ;
2016-09-23 22:57:35 +00:00
2017-03-13 23:06:52 +00:00
struct b3LinkState linkState ;
2016-09-23 22:57:35 +00:00
2017-03-13 23:06:52 +00:00
int bodyUniqueId = - 1 ;
int linkIndex = - 1 ;
2017-03-26 20:06:46 +00:00
int computeLinkVelocity = 0 ;
2017-03-13 23:06:52 +00:00
int i ;
b3PhysicsClientHandle sm = 0 ;
2016-09-23 22:57:35 +00:00
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
2017-03-26 20:06:46 +00:00
static char * kwlist [ ] = { " bodyUniqueId " , " linkIndex " , " computeLinkVelocity " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ii|ii " , kwlist , & bodyUniqueId , & linkIndex , & computeLinkVelocity , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-09-23 22:57:35 +00:00
2017-03-13 23:06:52 +00:00
{
{
int status_type = 0 ;
b3SharedMemoryCommandHandle cmd_handle ;
b3SharedMemoryStatusHandle status_handle ;
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
if ( bodyUniqueId < 0 )
{
PyErr_SetString ( SpamError , " getLinkState failed; invalid bodyIndex " ) ;
return NULL ;
}
if ( linkIndex < 0 )
{
PyErr_SetString ( SpamError , " getLinkState failed; invalid linkIndex " ) ;
return NULL ;
}
cmd_handle =
b3RequestActualStateCommandInit ( sm , bodyUniqueId ) ;
2017-03-26 20:06:46 +00:00
if ( computeLinkVelocity )
{
b3RequestActualStateCommandComputeLinkVelocity ( cmd_handle , computeLinkVelocity ) ;
}
2017-03-13 23:06:52 +00:00
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 ;
}
if ( 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 ] ) ) ;
}
pyLinkStateWorldLinkFramePosition = PyTuple_New ( 3 ) ;
for ( i = 0 ; i < 3 ; + + i )
{
PyTuple_SetItem ( pyLinkStateWorldLinkFramePosition , i ,
PyFloat_FromDouble ( linkState . m_worldLinkFramePosition [ i ] ) ) ;
}
pyLinkStateWorldLinkFrameOrientation = PyTuple_New ( 4 ) ;
for ( i = 0 ; i < 4 ; + + i )
{
PyTuple_SetItem ( pyLinkStateWorldLinkFrameOrientation , i ,
PyFloat_FromDouble ( linkState . m_worldLinkFrameOrientation [ i ] ) ) ;
}
2017-03-26 20:06:46 +00:00
if ( computeLinkVelocity )
{
pyLinkState = PyTuple_New ( 8 ) ;
} else
{
pyLinkState = PyTuple_New ( 6 ) ;
}
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( pyLinkState , 0 , pyLinkStateWorldPosition ) ;
PyTuple_SetItem ( pyLinkState , 1 , pyLinkStateWorldOrientation ) ;
PyTuple_SetItem ( pyLinkState , 2 , pyLinkStateLocalInertialPosition ) ;
PyTuple_SetItem ( pyLinkState , 3 , pyLinkStateLocalInertialOrientation ) ;
PyTuple_SetItem ( pyLinkState , 4 , pyLinkStateWorldLinkFramePosition ) ;
PyTuple_SetItem ( pyLinkState , 5 , pyLinkStateWorldLinkFrameOrientation ) ;
2017-03-26 20:06:46 +00:00
if ( computeLinkVelocity )
{
pyLinkStateWorldLinkLinearVelocity = PyTuple_New ( 3 ) ;
pyLinkStateWorldLinkAngularVelocity = PyTuple_New ( 3 ) ;
for ( i = 0 ; i < 3 ; + + i )
{
PyTuple_SetItem ( pyLinkStateWorldLinkLinearVelocity , i ,
PyFloat_FromDouble ( linkState . m_worldLinearVelocity [ i ] ) ) ;
PyTuple_SetItem ( pyLinkStateWorldLinkAngularVelocity , i ,
PyFloat_FromDouble ( linkState . m_worldAngularVelocity [ i ] ) ) ;
}
PyTuple_SetItem ( pyLinkState , 6 , pyLinkStateWorldLinkLinearVelocity ) ;
PyTuple_SetItem ( pyLinkState , 7 , pyLinkStateWorldLinkAngularVelocity ) ;
}
2017-03-13 23:06:52 +00:00
return pyLinkState ;
}
}
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-01-17 23:42:32 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_readUserDebugParameter ( PyObject * self , PyObject * args , PyObject * keywds )
{
2017-01-17 23:42:32 +00:00
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
int itemUniqueId ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " itemUniqueId " , " physicsClientId " , NULL } ;
2017-01-17 23:42:32 +00:00
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & itemUniqueId , & physicsClientId ) )
2017-01-17 23:42:32 +00:00
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2017-01-17 23:42:32 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
commandHandle = b3InitUserDebugReadParameter ( sm , itemUniqueId ) ;
2017-01-17 23:42:32 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
2017-03-13 23:06:52 +00:00
if ( statusType = = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED )
2017-01-17 23:42:32 +00:00
{
2017-03-13 23:06:52 +00:00
double paramValue = 0.f ;
int ok = b3GetStatusDebugParameterValue ( statusHandle , & paramValue ) ;
2017-01-17 23:42:32 +00:00
if ( ok )
{
PyObject * item = PyFloat_FromDouble ( paramValue ) ;
return item ;
}
}
PyErr_SetString ( SpamError , " Failed to read parameter. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_addUserDebugParameter ( PyObject * self , PyObject * args , PyObject * keywds )
{
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
char * text ;
double rangeMin = 0.f ;
double rangeMax = 1.f ;
double startValue = 0.f ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " paramName " , " rangeMin " , " rangeMax " , " startValue " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " s|dddi " , kwlist , & text , & rangeMin , & rangeMax , & startValue , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
commandHandle = b3InitUserDebugAddParameter ( sm , text , rangeMin , rangeMax , startValue ) ;
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 addUserDebugParameter. " ) ;
return NULL ;
}
2017-01-17 23:42:32 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_addUserDebugText ( PyObject * self , PyObject * args , PyObject * keywds )
2017-01-17 23:42:32 +00:00
{
2017-03-13 23:06:52 +00:00
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
int res = 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 ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " text " , " textPosition " , " textColorRGB " , " textSize " , " lifeTime " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " sO|Oddi " , kwlist , & text , & textPositionObj , & textColorRGBObj , & textSize , & lifeTime , & physicsClientId ) )
{
return NULL ;
}
2017-01-17 23:42:32 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
res = pybullet_internalSetVectord ( textPositionObj , posXYZ ) ;
if ( ! res )
2017-01-17 23:42:32 +00:00
{
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " Error converting textPositionObj[3] " ) ;
2017-01-17 23:42:32 +00:00
return NULL ;
}
2017-03-13 23:06:52 +00:00
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 )
2017-01-17 23:42:32 +00:00
{
int debugItemUniqueId = b3GetDebugItemUniqueId ( statusHandle ) ;
PyObject * item = PyInt_FromLong ( debugItemUniqueId ) ;
return item ;
}
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " Error in addUserDebugText. " ) ;
return NULL ;
2017-01-17 23:42:32 +00:00
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_addUserDebugLine ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-14 15:39:34 +00:00
{
2017-03-13 23:06:52 +00:00
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
int res = 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 ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " lineFromXYZ " , " lineToXYZ " , " lineColorRGB " , " lineWidth " , " lifeTime " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " OO|Oddi " , kwlist , & lineFromObj , & lineToObj , & lineColorRGBObj , & lineWidth , & lifeTime , & physicsClientId ) )
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
res = pybullet_internalSetVectord ( lineFromObj , fromXYZ ) ;
if ( ! res )
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " Error converting lineFrom[3] " ) ;
2016-12-28 04:25:52 +00:00
return NULL ;
}
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
res = pybullet_internalSetVectord ( lineToObj , toXYZ ) ;
if ( ! res )
{
PyErr_SetString ( SpamError , " Error converting lineTo[3] " ) ;
return NULL ;
}
if ( lineColorRGBObj )
{
res = pybullet_internalSetVectord ( lineColorRGBObj , colorRGB ) ;
}
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
commandHandle = b3InitUserDebugDrawAddLine3D ( sm , fromXYZ , toXYZ , colorRGB , lineWidth , lifeTime ) ;
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
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 ;
}
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " Error in addUserDebugLine. " ) ;
return NULL ;
}
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_removeUserDebugItem ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-14 15:39:34 +00:00
{
2017-03-13 23:06:52 +00:00
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
int itemUniqueId ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2016-12-28 04:25:52 +00:00
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " itemUniqueId " , " physicsClientId " , NULL } ;
2016-12-28 04:25:52 +00:00
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & itemUniqueId , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-11-14 15:39:34 +00:00
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
commandHandle = b3InitUserDebugDrawRemove ( sm , itemUniqueId ) ;
2016-11-14 15:39:34 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_removeAllUserDebugItems ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-14 15:39:34 +00:00
{
2017-03-13 23:06:52 +00:00
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |i " , kwlist , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-11-14 15:39:34 +00:00
commandHandle = b3InitUserDebugDrawRemoveAll ( sm ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_startStateLogging ( PyObject * self , PyObject * args , PyObject * keywds )
2017-02-17 18:47:55 +00:00
{
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
b3PhysicsClientHandle sm = 0 ;
int loggingType = - 1 ;
char * fileName = 0 ;
PyObject * objectUniqueIdsObj = 0 ;
2017-03-13 23:06:52 +00:00
int maxLogDof = - 1 ;
2017-04-04 17:38:25 +00:00
int bodyUniqueIdA = - 1 ;
int bodyUniqueIdB = - 1 ;
2017-04-02 23:03:20 +00:00
int linkIndexA = - 2 ;
int linkIndexB = - 2 ;
2017-04-08 18:48:12 +00:00
int deviceTypeFilter = - 1 ;
2017-02-17 18:47:55 +00:00
2017-04-08 18:48:12 +00:00
static char * kwlist [ ] = { " loggingType " , " fileName " , " objectUniqueIds " , " maxLogDof " , " bodyUniqueIdA " , " bodyUniqueIdB " , " linkIndexA " , " linkIndexB " , " deviceTypeFilter " , " physicsClientId " , NULL } ;
2017-02-17 18:47:55 +00:00
int physicsClientId = 0 ;
2017-03-13 23:06:52 +00:00
2017-04-12 22:02:47 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " is|Oiiiiiii " , kwlist ,
& loggingType , & fileName , & objectUniqueIdsObj , & maxLogDof , & bodyUniqueIdA , & bodyUniqueIdB , & linkIndexA , & linkIndexB , & deviceTypeFilter , & physicsClientId ) )
2017-02-17 18:47:55 +00:00
return NULL ;
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2017-02-17 18:47:55 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-02-17 22:25:53 +00:00
{
b3SharedMemoryCommandHandle commandHandle ;
commandHandle = b3StateLoggingCommandInit ( sm ) ;
2017-03-13 23:06:52 +00:00
b3StateLoggingStart ( commandHandle , loggingType , fileName ) ;
2017-02-17 22:25:53 +00:00
if ( objectUniqueIdsObj )
{
PyObject * seq = PySequence_Fast ( objectUniqueIdsObj , " expected a sequence of object unique ids " ) ;
if ( seq )
{
int len = PySequence_Size ( objectUniqueIdsObj ) ;
2017-02-17 23:15:13 +00:00
int i ;
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < len ; i + + )
2017-02-17 22:25:53 +00:00
{
int objectUid = pybullet_internalGetFloatFromSequence ( seq , i ) ;
2017-03-13 23:06:52 +00:00
b3StateLoggingAddLoggingObjectUniqueId ( commandHandle , objectUid ) ;
2017-02-17 22:25:53 +00:00
}
Py_DECREF ( seq ) ;
}
}
2017-03-13 23:06:52 +00:00
if ( maxLogDof > 0 )
2017-03-04 23:30:57 +00:00
{
b3StateLoggingSetMaxLogDof ( commandHandle , maxLogDof ) ;
}
2017-04-02 23:03:20 +00:00
2017-04-04 17:38:25 +00:00
if ( bodyUniqueIdA > - 1 )
2017-04-02 23:03:20 +00:00
{
2017-04-04 17:38:25 +00:00
b3StateLoggingSetBodyAUniqueId ( commandHandle , bodyUniqueIdA ) ;
2017-04-02 23:03:20 +00:00
}
2017-04-04 17:38:25 +00:00
if ( bodyUniqueIdB > - 1 )
2017-04-02 23:03:20 +00:00
{
2017-04-04 17:38:25 +00:00
b3StateLoggingSetBodyBUniqueId ( commandHandle , bodyUniqueIdB ) ;
2017-04-02 23:03:20 +00:00
}
if ( linkIndexA > - 2 )
{
b3StateLoggingSetLinkIndexA ( commandHandle , linkIndexA ) ;
}
if ( linkIndexB > - 2 )
{
b3StateLoggingSetLinkIndexB ( commandHandle , linkIndexB ) ;
}
2017-03-13 23:06:52 +00:00
2017-04-08 18:48:12 +00:00
if ( deviceTypeFilter > = 0 )
{
b3StateLoggingSetDeviceTypeFilter ( commandHandle , deviceTypeFilter ) ;
}
2017-02-17 22:25:53 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
2017-03-13 23:06:52 +00:00
if ( statusType = = CMD_STATE_LOGGING_START_COMPLETED )
2017-02-17 22:25:53 +00:00
{
int loggingUniqueId = b3GetStatusLoggingUniqueId ( statusHandle ) ;
PyObject * loggingUidObj = PyInt_FromLong ( loggingUniqueId ) ;
return loggingUidObj ;
}
}
Py_INCREF ( Py_None ) ;
return Py_None ;
2017-02-17 18:47:55 +00:00
}
2017-05-05 00:51:40 +00:00
static PyObject * pybullet_submitProfileTiming ( PyObject * self , PyObject * args , PyObject * keywds )
{
2017-05-10 22:01:25 +00:00
// b3SharedMemoryStatusHandle statusHandle;
// int statusType;
2017-05-05 00:51:40 +00:00
char * eventName = 0 ;
int duractionInMicroSeconds = - 1 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " eventName " , " duraction " , " physicsClientId " , NULL } ;
int physicsClientId = 0 ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " s|ii " , kwlist ,
& eventName , & duractionInMicroSeconds , & physicsClientId ) )
return NULL ;
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
if ( eventName )
{
b3SharedMemoryCommandHandle commandHandle ;
commandHandle = b3ProfileTimingCommandInit ( sm , eventName ) ;
if ( duractionInMicroSeconds > = 0 )
{
b3SetProfileTimingDuractionInMicroSeconds ( commandHandle , duractionInMicroSeconds ) ;
}
b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_stopStateLogging ( PyObject * self , PyObject * args , PyObject * keywds )
2017-02-17 18:47:55 +00:00
{
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
2017-03-13 23:06:52 +00:00
int loggingId = - 1 ;
2017-02-17 22:25:53 +00:00
2017-02-17 18:47:55 +00:00
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " loggingId " , " physicsClientId " , NULL } ;
int physicsClientId = 0 ;
2017-02-17 22:25:53 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist ,
2017-03-13 23:06:52 +00:00
& loggingId , & physicsClientId ) )
2017-02-17 18:47:55 +00:00
return NULL ;
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2017-02-17 18:47:55 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
if ( loggingId > = 0 )
2017-02-17 22:25:53 +00:00
{
b3SharedMemoryCommandHandle commandHandle ;
commandHandle = b3StateLoggingCommandInit ( sm ) ;
b3StateLoggingStop ( commandHandle , loggingId ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
2017-02-17 18:47:55 +00:00
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_setTimeOut ( PyObject * self , PyObject * args , PyObject * keywds )
2017-02-24 23:34:11 +00:00
{
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " timeOutInSeconds " , " physicsClientId " , NULL } ;
2017-02-24 23:34:11 +00:00
double timeOutInSeconds = - 1 ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " d|i " , kwlist ,
2017-03-13 23:06:52 +00:00
& timeOutInSeconds , & physicsClientId ) )
2017-02-24 23:34:11 +00:00
return NULL ;
2017-03-13 23:06:52 +00:00
if ( timeOutInSeconds > = 0 )
2017-02-24 23:34:11 +00:00
{
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2017-02-24 23:34:11 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
b3SetTimeOut ( sm , timeOutInSeconds ) ;
2017-02-24 23:34:11 +00:00
}
2017-02-17 18:47:55 +00:00
2017-02-24 23:34:11 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2016-12-27 03:40:09 +00:00
2017-04-05 22:21:26 +00:00
static PyObject * pybullet_rayTestObsolete ( PyObject * self , PyObject * args , PyObject * keywds )
2016-12-27 03:40:09 +00:00
{
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
2017-03-13 23:06:52 +00:00
PyObject * rayFromObj = 0 ;
PyObject * rayToObj = 0 ;
2016-12-27 03:40:09 +00:00
double from [ 3 ] ;
double to [ 3 ] ;
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " rayFromPosition " , " rayToPosition " , " physicsClientId " , NULL } ;
int physicsClientId = 0 ;
2016-12-28 04:25:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " OO|i " , kwlist ,
2017-03-13 23:06:52 +00:00
& rayFromObj , & rayToObj , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
return NULL ;
2016-12-27 03:40:09 +00:00
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
2016-12-27 03:40:09 +00:00
return NULL ;
2016-12-28 04:25:52 +00:00
}
2016-12-27 03:40:09 +00:00
2017-03-13 23:06:52 +00:00
pybullet_internalSetVectord ( rayFromObj , from ) ;
pybullet_internalSetVectord ( rayToObj , to ) ;
2016-12-27 03:40:09 +00:00
2017-03-13 23:06:52 +00:00
commandHandle = b3CreateRaycastCommandInit ( sm , from [ 0 ] , from [ 1 ] , from [ 2 ] ,
to [ 0 ] , to [ 1 ] , to [ 2 ] ) ;
2016-12-27 03:40:09 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
2017-03-13 23:06:52 +00:00
if ( statusType = = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED )
2016-12-27 03:40:09 +00:00
{
struct b3RaycastInformation raycastInfo ;
PyObject * rayHitsObj = 0 ;
int i ;
b3GetRaycastInformation ( sm , & raycastInfo ) ;
2017-03-13 23:06:52 +00:00
2016-12-27 03:40:09 +00:00
rayHitsObj = PyTuple_New ( raycastInfo . m_numRayHits ) ;
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < raycastInfo . m_numRayHits ; i + + )
2016-12-27 03:40:09 +00:00
{
2017-03-13 23:06:52 +00:00
PyObject * singleHitObj = PyTuple_New ( 5 ) ;
2016-12-27 03:40:09 +00:00
{
PyObject * ob = PyInt_FromLong ( raycastInfo . m_rayHits [ i ] . m_hitObjectUniqueId ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( singleHitObj , 0 , ob ) ;
2016-12-27 03:40:09 +00:00
}
{
PyObject * ob = PyInt_FromLong ( raycastInfo . m_rayHits [ i ] . m_hitObjectLinkIndex ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( singleHitObj , 1 , ob ) ;
2016-12-27 03:40:09 +00:00
}
{
PyObject * ob = PyFloat_FromDouble ( raycastInfo . m_rayHits [ i ] . m_hitFraction ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( singleHitObj , 2 , ob ) ;
2016-12-27 03:40:09 +00:00
}
{
2017-03-13 23:06:52 +00:00
PyObject * posObj = PyTuple_New ( 3 ) ;
2016-12-27 03:40:09 +00:00
int p ;
2017-03-13 23:06:52 +00:00
for ( p = 0 ; p < 3 ; p + + )
2016-12-27 03:40:09 +00:00
{
PyObject * ob = PyFloat_FromDouble ( raycastInfo . m_rayHits [ i ] . m_hitPositionWorld [ p ] ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( posObj , p , ob ) ;
2016-12-27 03:40:09 +00:00
}
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( singleHitObj , 3 , posObj ) ;
2016-12-27 03:40:09 +00:00
}
{
2017-03-13 23:06:52 +00:00
PyObject * normalObj = PyTuple_New ( 3 ) ;
2016-12-27 03:40:09 +00:00
int p ;
2017-03-13 23:06:52 +00:00
for ( p = 0 ; p < 3 ; p + + )
2016-12-27 03:40:09 +00:00
{
PyObject * ob = PyFloat_FromDouble ( raycastInfo . m_rayHits [ i ] . m_hitNormalWorld [ p ] ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( normalObj , p , ob ) ;
2016-12-27 03:40:09 +00:00
}
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( singleHitObj , 4 , normalObj ) ;
2016-12-27 03:40:09 +00:00
}
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( rayHitsObj , i , singleHitObj ) ;
2016-12-27 03:40:09 +00:00
}
return rayHitsObj ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-04-05 22:21:26 +00:00
static PyObject * pybullet_rayTestBatch ( PyObject * self , PyObject * args , PyObject * keywds )
{
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
PyObject * rayFromObjList = 0 ;
PyObject * rayToObjList = 0 ;
b3PhysicsClientHandle sm = 0 ;
int sizeFrom = 0 ;
int sizeTo = 0 ;
static char * kwlist [ ] = { " rayFromPositions " , " rayToPositions " , " physicsClientId " , NULL } ;
int physicsClientId = 0 ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " OO|i " , kwlist ,
& rayFromObjList , & rayToObjList , & physicsClientId ) )
return NULL ;
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
commandHandle = b3CreateRaycastBatchCommandInit ( sm ) ;
if ( rayFromObjList )
{
PyObject * seqRayFromObj = PySequence_Fast ( rayFromObjList , " expected a sequence of rayFrom positions " ) ;
PyObject * seqRayToObj = PySequence_Fast ( rayToObjList , " expected a sequence of 'rayTo' positions " ) ;
if ( seqRayFromObj & & seqRayToObj )
{
int lenFrom = PySequence_Size ( rayFromObjList ) ;
int lenTo = PySequence_Size ( seqRayToObj ) ;
if ( lenFrom ! = lenTo )
{
PyErr_SetString ( SpamError , " Size of from_positions need to be equal to size of to_positions. " ) ;
Py_DECREF ( seqRayFromObj ) ;
Py_DECREF ( seqRayToObj ) ;
return NULL ;
} else
{
int i ;
if ( lenFrom > = MAX_RAY_INTERSECTION_BATCH_SIZE )
{
PyErr_SetString ( SpamError , " Number of rays exceed the maximum batch size. " ) ;
Py_DECREF ( seqRayFromObj ) ;
Py_DECREF ( seqRayToObj ) ;
return NULL ;
}
for ( i = 0 ; i < lenFrom ; i + + )
{
PyObject * rayFromObj = PySequence_GetItem ( rayFromObjList , i ) ;
PyObject * rayToObj = PySequence_GetItem ( seqRayToObj , i ) ;
double rayFromWorld [ 3 ] ;
double rayToWorld [ 3 ] ;
if ( ( pybullet_internalSetVectord ( rayFromObj , rayFromWorld ) ) & &
( pybullet_internalSetVectord ( rayToObj , rayToWorld ) ) )
{
b3RaycastBatchAddRay ( commandHandle , rayFromWorld , rayToWorld ) ;
} else
{
PyErr_SetString ( SpamError , " Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles " ) ;
Py_DECREF ( seqRayFromObj ) ;
Py_DECREF ( seqRayToObj ) ;
Py_DECREF ( rayFromObj ) ;
Py_DECREF ( rayToObj ) ;
return NULL ;
}
Py_DECREF ( rayFromObj ) ;
Py_DECREF ( rayToObj ) ;
}
}
} else
{
}
if ( seqRayFromObj )
{
Py_DECREF ( seqRayFromObj ) ;
}
if ( seqRayToObj )
{
Py_DECREF ( seqRayToObj ) ;
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
if ( statusType = = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED )
{
struct b3RaycastInformation raycastInfo ;
PyObject * rayHitsObj = 0 ;
int i ;
b3GetRaycastInformation ( sm , & raycastInfo ) ;
rayHitsObj = PyTuple_New ( raycastInfo . m_numRayHits ) ;
for ( i = 0 ; i < raycastInfo . m_numRayHits ; i + + )
{
PyObject * singleHitObj = PyTuple_New ( 5 ) ;
{
PyObject * ob = PyInt_FromLong ( raycastInfo . m_rayHits [ i ] . m_hitObjectUniqueId ) ;
PyTuple_SetItem ( singleHitObj , 0 , ob ) ;
}
{
PyObject * ob = PyInt_FromLong ( raycastInfo . m_rayHits [ i ] . m_hitObjectLinkIndex ) ;
PyTuple_SetItem ( singleHitObj , 1 , ob ) ;
}
{
PyObject * ob = PyFloat_FromDouble ( raycastInfo . m_rayHits [ i ] . m_hitFraction ) ;
PyTuple_SetItem ( singleHitObj , 2 , ob ) ;
}
{
PyObject * posObj = PyTuple_New ( 3 ) ;
int p ;
for ( p = 0 ; p < 3 ; p + + )
{
PyObject * ob = PyFloat_FromDouble ( raycastInfo . m_rayHits [ i ] . m_hitPositionWorld [ p ] ) ;
PyTuple_SetItem ( posObj , p , ob ) ;
}
PyTuple_SetItem ( singleHitObj , 3 , posObj ) ;
}
{
PyObject * normalObj = PyTuple_New ( 3 ) ;
int p ;
for ( p = 0 ; p < 3 ; p + + )
{
PyObject * ob = PyFloat_FromDouble ( raycastInfo . m_rayHits [ i ] . m_hitNormalWorld [ p ] ) ;
PyTuple_SetItem ( normalObj , p , ob ) ;
}
PyTuple_SetItem ( singleHitObj , 4 , normalObj ) ;
}
PyTuple_SetItem ( rayHitsObj , i , singleHitObj ) ;
}
return rayHitsObj ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-02-03 16:34:22 +00:00
static PyObject * pybullet_getMatrixFromQuaternion ( PyObject * self , PyObject * args )
2016-12-27 03:40:09 +00:00
{
PyObject * quatObj ;
double quat [ 4 ] ;
2017-03-13 23:06:52 +00:00
if ( PyArg_ParseTuple ( args , " O " , & quatObj ) )
2016-12-27 03:40:09 +00:00
{
2017-03-13 23:06:52 +00:00
if ( pybullet_internalSetVector4d ( quatObj , quat ) )
2016-12-27 03:40:09 +00:00
{
///see btMatrix3x3::setRotation
int i ;
2017-03-13 23:06:52 +00:00
double d = quat [ 0 ] * quat [ 0 ] + quat [ 1 ] * quat [ 1 ] + quat [ 2 ] * quat [ 2 ] + quat [ 3 ] * quat [ 3 ] ;
2016-12-27 03:40:09 +00:00
double s = 2.0 / d ;
2017-03-13 23:06:52 +00:00
double xs = quat [ 0 ] * s , ys = quat [ 1 ] * s , zs = quat [ 2 ] * s ;
double wx = quat [ 3 ] * xs , wy = quat [ 3 ] * ys , wz = quat [ 3 ] * zs ;
double xx = quat [ 0 ] * xs , xy = quat [ 0 ] * ys , xz = quat [ 0 ] * zs ;
double yy = quat [ 1 ] * ys , yz = quat [ 1 ] * zs , zz = quat [ 2 ] * zs ;
2016-12-27 03:40:09 +00:00
double mat3x3 [ 9 ] = {
1.0 - ( yy + zz ) , xy - wz , xz + wy ,
xy + wz , 1.0 - ( xx + zz ) , yz - wx ,
xz - wy , yz + wx , 1.0 - ( xx + yy ) } ;
PyObject * matObj = PyTuple_New ( 9 ) ;
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < 9 ; i + + )
2016-12-27 03:40:09 +00:00
{
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( matObj , i , PyFloat_FromDouble ( mat3x3 [ i ] ) ) ;
2016-12-27 03:40:09 +00:00
}
return matObj ;
}
}
PyErr_SetString ( SpamError , " Couldn't convert quaternion [x,y,z,w]. " ) ;
return NULL ;
} ;
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_setVRCameraState ( PyObject * self , PyObject * args , PyObject * keywds )
2017-01-06 01:41:58 +00:00
{
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
PyObject * rootPosObj = 0 ;
PyObject * rootOrnObj = 0 ;
int trackObjectUid = - 2 ;
2017-01-06 01:41:58 +00:00
double rootPos [ 3 ] ;
double rootOrn [ 4 ] ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " rootPosition " , " rootOrientation " , " trackObject " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |OOii " , kwlist , & rootPosObj , & rootOrnObj , & trackObjectUid , & physicsClientId ) )
2017-01-06 01:41:58 +00:00
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2017-01-06 01:41:58 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
commandHandle = b3SetVRCameraStateCommandInit ( sm ) ;
2017-03-13 23:06:52 +00:00
if ( pybullet_internalSetVectord ( rootPosObj , rootPos ) )
2017-01-06 01:41:58 +00:00
{
2017-03-13 23:06:52 +00:00
b3SetVRCameraRootPosition ( commandHandle , rootPos ) ;
2017-01-06 01:41:58 +00:00
}
2017-03-13 23:06:52 +00:00
if ( pybullet_internalSetVector4d ( rootOrnObj , rootOrn ) )
2017-01-06 01:41:58 +00:00
{
2017-03-13 23:06:52 +00:00
b3SetVRCameraRootOrientation ( commandHandle , rootOrn ) ;
2017-01-06 01:41:58 +00:00
}
2017-03-13 23:06:52 +00:00
if ( trackObjectUid > = - 1 )
2017-01-06 01:41:58 +00:00
{
2017-03-13 23:06:52 +00:00
b3SetVRCameraTrackingObject ( commandHandle , trackObjectUid ) ;
2017-01-06 01:41:58 +00:00
}
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getKeyboardEvents ( PyObject * self , PyObject * args , PyObject * keywds )
2017-03-02 20:33:22 +00:00
{
b3SharedMemoryCommandHandle commandHandle ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
struct b3KeyboardEventsData keyboardEventsData ;
PyObject * keyEventsObj = 0 ;
2017-03-13 23:06:52 +00:00
int i = 0 ;
2017-03-02 20:33:22 +00:00
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |i " , kwlist , & physicsClientId ) )
2017-03-02 20:33:22 +00:00
{
return NULL ;
}
2017-03-13 23:06:52 +00:00
2017-03-02 20:33:22 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2017-03-02 20:33:22 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
commandHandle = b3RequestKeyboardEventsCommandInit ( sm ) ;
b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
2017-03-13 23:06:52 +00:00
b3GetKeyboardEventsData ( sm , & keyboardEventsData ) ;
2017-03-02 20:33:22 +00:00
keyEventsObj = PyDict_New ( ) ;
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < keyboardEventsData . m_numKeyboardEvents ; i + + )
2017-03-02 20:33:22 +00:00
{
PyObject * keyObj = PyLong_FromLong ( keyboardEventsData . m_keyboardEvents [ i ] . m_keyCode ) ;
PyObject * valObj = PyLong_FromLong ( keyboardEventsData . m_keyboardEvents [ i ] . m_keyState ) ;
PyDict_SetItem ( keyEventsObj , keyObj , valObj ) ;
}
return keyEventsObj ;
}
2017-01-06 01:41:58 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getVREvents ( PyObject * self , PyObject * args , PyObject * keywds )
2016-12-27 03:40:09 +00:00
{
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
2017-04-08 05:53:36 +00:00
int deviceTypeFilter = VR_DEVICE_CONTROLLER ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-04-08 05:53:36 +00:00
static char * kwlist [ ] = { " deviceTypeFilter " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |ii " , kwlist , & deviceTypeFilter , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2017-03-13 23:06:52 +00:00
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-12-27 03:40:09 +00:00
commandHandle = b3RequestVREventsCommandInit ( sm ) ;
2017-04-08 05:53:36 +00:00
b3VREventsSetDeviceTypeFilter ( commandHandle , deviceTypeFilter ) ;
2016-12-27 03:40:09 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
2017-03-13 23:06:52 +00:00
if ( statusType = = CMD_REQUEST_VR_EVENTS_DATA_COMPLETED )
2016-12-27 03:40:09 +00:00
{
struct b3VREventsData vrEvents ;
PyObject * vrEventsObj ;
2017-03-13 23:06:52 +00:00
int i = 0 ;
b3GetVREventsData ( sm , & vrEvents ) ;
2016-12-27 03:40:09 +00:00
vrEventsObj = PyTuple_New ( vrEvents . m_numControllerEvents ) ;
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < vrEvents . m_numControllerEvents ; i + + )
2016-12-27 03:40:09 +00:00
{
2017-04-08 05:53:36 +00:00
PyObject * vrEventObj = PyTuple_New ( 8 ) ;
2016-12-27 03:40:09 +00:00
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( vrEventObj , 0 , PyInt_FromLong ( vrEvents . m_controllerEvents [ i ] . m_controllerId ) ) ;
2016-12-27 03:40:09 +00:00
{
PyObject * posObj = PyTuple_New ( 3 ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( posObj , 0 , PyFloat_FromDouble ( vrEvents . m_controllerEvents [ i ] . m_pos [ 0 ] ) ) ;
PyTuple_SetItem ( posObj , 1 , PyFloat_FromDouble ( vrEvents . m_controllerEvents [ i ] . m_pos [ 1 ] ) ) ;
PyTuple_SetItem ( posObj , 2 , PyFloat_FromDouble ( vrEvents . m_controllerEvents [ i ] . m_pos [ 2 ] ) ) ;
PyTuple_SetItem ( vrEventObj , 1 , posObj ) ;
2016-12-27 03:40:09 +00:00
}
{
PyObject * ornObj = PyTuple_New ( 4 ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( ornObj , 0 , PyFloat_FromDouble ( vrEvents . m_controllerEvents [ i ] . m_orn [ 0 ] ) ) ;
PyTuple_SetItem ( ornObj , 1 , PyFloat_FromDouble ( vrEvents . m_controllerEvents [ i ] . m_orn [ 1 ] ) ) ;
PyTuple_SetItem ( ornObj , 2 , PyFloat_FromDouble ( vrEvents . m_controllerEvents [ i ] . m_orn [ 2 ] ) ) ;
PyTuple_SetItem ( ornObj , 3 , PyFloat_FromDouble ( vrEvents . m_controllerEvents [ i ] . m_orn [ 3 ] ) ) ;
PyTuple_SetItem ( vrEventObj , 2 , ornObj ) ;
2016-12-27 03:40:09 +00:00
}
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( vrEventObj , 3 , PyFloat_FromDouble ( vrEvents . m_controllerEvents [ i ] . m_analogAxis ) ) ;
PyTuple_SetItem ( vrEventObj , 4 , PyInt_FromLong ( vrEvents . m_controllerEvents [ i ] . m_numButtonEvents ) ) ;
PyTuple_SetItem ( vrEventObj , 5 , PyInt_FromLong ( vrEvents . m_controllerEvents [ i ] . m_numMoveEvents ) ) ;
2016-12-27 03:40:09 +00:00
{
PyObject * buttonsObj = PyTuple_New ( MAX_VR_BUTTONS ) ;
int b ;
2017-03-13 23:06:52 +00:00
for ( b = 0 ; b < MAX_VR_BUTTONS ; b + + )
2016-12-27 03:40:09 +00:00
{
PyObject * button = PyInt_FromLong ( vrEvents . m_controllerEvents [ i ] . m_buttons [ b ] ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( buttonsObj , b , button ) ;
2016-12-27 03:40:09 +00:00
}
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( vrEventObj , 6 , buttonsObj ) ;
2016-12-27 03:40:09 +00:00
}
2017-04-08 05:53:36 +00:00
PyTuple_SetItem ( vrEventObj , 7 , PyInt_FromLong ( vrEvents . m_controllerEvents [ i ] . m_deviceType ) ) ;
2017-03-13 23:06:52 +00:00
PyTuple_SetItem ( vrEventsObj , i , vrEventObj ) ;
2016-12-27 03:40:09 +00:00
}
2017-03-13 23:06:52 +00:00
return vrEventsObj ;
2016-12-27 03:40:09 +00:00
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-04-10 18:03:41 +00:00
static PyObject * pybullet_getDebugVisualizerCamera ( PyObject * self , PyObject * args , PyObject * keywds )
{
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " physicsClientId " , NULL } ;
b3SharedMemoryCommandHandle commandHandle ;
int hasCamInfo ;
b3SharedMemoryStatusHandle statusHandle ;
struct b3OpenGLVisualizerCameraInfo camera ;
PyObject * pyCameraList = 0 ;
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
commandHandle = b3InitRequestOpenGLVisualizerCameraCommand ( sm ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
hasCamInfo = b3GetStatusOpenGLVisualizerCamera ( statusHandle , & camera ) ;
if ( hasCamInfo )
{
PyObject * item = 0 ;
pyCameraList = PyTuple_New ( 8 ) ;
item = PyInt_FromLong ( camera . m_width ) ;
PyTuple_SetItem ( pyCameraList , 0 , item ) ;
item = PyInt_FromLong ( camera . m_height ) ;
PyTuple_SetItem ( pyCameraList , 1 , item ) ;
{
PyObject * viewMat16 = PyTuple_New ( 16 ) ;
PyObject * projMat16 = PyTuple_New ( 16 ) ;
int i ;
for ( i = 0 ; i < 16 ; i + + )
{
item = PyFloat_FromDouble ( camera . m_viewMatrix [ i ] ) ;
PyTuple_SetItem ( viewMat16 , i , item ) ;
item = PyFloat_FromDouble ( camera . m_projectionMatrix [ i ] ) ;
PyTuple_SetItem ( projMat16 , i , item ) ;
}
PyTuple_SetItem ( pyCameraList , 2 , viewMat16 ) ;
PyTuple_SetItem ( pyCameraList , 3 , projMat16 ) ;
}
{
PyObject * item = 0 ;
int i ;
PyObject * camUp = PyTuple_New ( 3 ) ;
PyObject * camFwd = PyTuple_New ( 3 ) ;
PyObject * hor = PyTuple_New ( 3 ) ;
PyObject * vert = PyTuple_New ( 3 ) ;
for ( i = 0 ; i < 3 ; i + + )
{
item = PyFloat_FromDouble ( camera . m_camUp [ i ] ) ;
PyTuple_SetItem ( camUp , i , item ) ;
item = PyFloat_FromDouble ( camera . m_camForward [ i ] ) ;
PyTuple_SetItem ( camFwd , i , item ) ;
item = PyFloat_FromDouble ( camera . m_horizontal [ i ] ) ;
PyTuple_SetItem ( hor , i , item ) ;
item = PyFloat_FromDouble ( camera . m_vertical [ i ] ) ;
PyTuple_SetItem ( vert , i , item ) ;
}
PyTuple_SetItem ( pyCameraList , 4 , camUp ) ;
PyTuple_SetItem ( pyCameraList , 5 , camFwd ) ;
PyTuple_SetItem ( pyCameraList , 6 , hor ) ;
PyTuple_SetItem ( pyCameraList , 7 , vert ) ;
}
return pyCameraList ;
}
PyErr_SetString ( SpamError , " Cannot get OpenGL visualizer camera info. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_configureDebugVisualizer ( PyObject * self , PyObject * args , PyObject * keywds )
2017-02-22 01:36:54 +00:00
{
2017-03-13 23:06:52 +00:00
int flag = 1 ;
int enable = - 1 ;
2017-02-22 01:36:54 +00:00
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " flag " , " enable " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ii|i " , kwlist ,
& flag , & enable , & physicsClientId ) )
return NULL ;
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer ( sm ) ;
b3ConfigureOpenGLVisualizerSetVisualizationFlags ( commandHandle , flag , enable ) ;
b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-02-22 01:36:54 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_resetDebugVisualizerCamera ( PyObject * self , PyObject * args , PyObject * keywds )
2017-03-01 23:04:07 +00:00
{
2017-03-13 23:06:52 +00:00
float cameraDistance = - 1 ;
2017-03-13 17:38:02 +00:00
float cameraYaw = 35 ;
float cameraPitch = 50 ;
2017-03-13 23:06:52 +00:00
PyObject * cameraTargetPosObj = 0 ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " cameraDistance " , " cameraYaw " , " cameraPitch " , " cameraTargetPosition " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " fffO|i " , kwlist ,
& cameraDistance , & cameraYaw , & cameraPitch , & cameraTargetPosObj , & physicsClientId ) )
return NULL ;
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer ( sm ) ;
2017-03-14 01:50:32 +00:00
if ( ( cameraDistance > = 0 ) )
2017-03-01 23:04:07 +00:00
{
float cameraTargetPosition [ 3 ] ;
2017-03-13 23:06:52 +00:00
if ( pybullet_internalSetVector ( cameraTargetPosObj , cameraTargetPosition ) )
2017-03-01 23:04:07 +00:00
{
2017-03-13 23:06:52 +00:00
b3ConfigureOpenGLVisualizerSetViewMatrix ( commandHandle , cameraDistance , cameraPitch , cameraYaw , cameraTargetPosition ) ;
2017-03-01 23:04:07 +00:00
}
}
2017-03-13 23:06:52 +00:00
b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
}
Py_INCREF ( Py_None ) ;
return Py_None ;
2017-03-01 23:04:07 +00:00
}
2016-12-27 03:40:09 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_setDebugObjectColor ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-21 15:42:11 +00:00
{
PyObject * objectColorRGBObj = 0 ;
double objectColorRGB [ 3 ] ;
int objectUniqueId = - 1 ;
int linkIndex = - 2 ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
2017-03-13 23:06:52 +00:00
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " objectUniqueId " , " linkIndex " , " objectDebugColorRGB " , " physicsClientId " , NULL } ;
2016-11-21 15:42:11 +00:00
2016-12-28 04:25:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ii|Oi " , kwlist ,
2017-03-13 23:06:52 +00:00
& objectUniqueId , & linkIndex , & objectColorRGBObj , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
return NULL ;
2016-11-21 15:42:11 +00:00
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
2016-11-21 15:42:11 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
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 ;
}
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getVisualShapeData ( PyObject * self , PyObject * args , PyObject * keywds )
2016-10-19 05:05:28 +00:00
{
int objectUniqueId = - 1 ;
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
struct b3VisualShapeInformation visualShapeInfo ;
int statusType ;
2016-10-19 14:42:55 +00:00
int i ;
2016-10-19 05:05:28 +00:00
PyObject * pyResultList = 0 ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " objectUniqueId " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & objectUniqueId , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
2016-11-10 05:01:04 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-12-28 04:25:52 +00:00
2016-10-19 05:05:28 +00:00
{
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 ) ;
2016-10-19 14:42:55 +00:00
for ( i = 0 ; i < visualShapeInfo . m_numVisualShapes ; i + + )
2016-10-19 05:05:28 +00:00
{
2017-02-03 16:34:22 +00:00
PyObject * visualShapeObList = PyTuple_New ( 8 ) ;
2016-10-19 05:05:28 +00:00
PyObject * item ;
item = PyInt_FromLong ( visualShapeInfo . m_visualShapeData [ i ] . m_objectUniqueId ) ;
PyTuple_SetItem ( visualShapeObList , 0 , item ) ;
2017-03-13 23:06:52 +00:00
2016-10-19 05:05:28 +00:00
item = PyInt_FromLong ( visualShapeInfo . m_visualShapeData [ i ] . m_linkIndex ) ;
PyTuple_SetItem ( visualShapeObList , 1 , item ) ;
2017-03-13 23:06:52 +00:00
2016-10-19 05:05:28 +00:00
item = PyInt_FromLong ( visualShapeInfo . m_visualShapeData [ i ] . m_visualGeometryType ) ;
PyTuple_SetItem ( visualShapeObList , 2 , item ) ;
{
PyObject * vec = PyTuple_New ( 3 ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_dimensions [ 0 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 0 , item ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_dimensions [ 1 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 1 , item ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_dimensions [ 2 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 2 , item ) ;
PyTuple_SetItem ( visualShapeObList , 3 , vec ) ;
}
2017-03-13 23:06:52 +00:00
2016-10-19 05:05:28 +00:00
item = PyString_FromString ( visualShapeInfo . m_visualShapeData [ i ] . m_meshAssetFileName ) ;
PyTuple_SetItem ( visualShapeObList , 4 , item ) ;
{
PyObject * vec = PyTuple_New ( 3 ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_localVisualFrame [ 0 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 0 , item ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_localVisualFrame [ 1 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 1 , item ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_localVisualFrame [ 2 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 2 , item ) ;
PyTuple_SetItem ( visualShapeObList , 5 , vec ) ;
}
{
PyObject * vec = PyTuple_New ( 4 ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_localVisualFrame [ 3 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 0 , item ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_localVisualFrame [ 4 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 1 , item ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_localVisualFrame [ 5 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 2 , item ) ;
2017-02-03 16:34:22 +00:00
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_localVisualFrame [ 6 ] ) ;
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( vec , 3 , item ) ;
PyTuple_SetItem ( visualShapeObList , 6 , vec ) ;
}
2017-03-13 23:06:52 +00:00
{
PyObject * rgba = PyTuple_New ( 4 ) ;
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_rgbaColor [ 0 ] ) ;
PyTuple_SetItem ( rgba , 0 , item ) ;
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_rgbaColor [ 1 ] ) ;
PyTuple_SetItem ( rgba , 1 , item ) ;
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_rgbaColor [ 2 ] ) ;
PyTuple_SetItem ( rgba , 2 , item ) ;
item = PyFloat_FromDouble ( visualShapeInfo . m_visualShapeData [ i ] . m_rgbaColor [ 3 ] ) ;
PyTuple_SetItem ( rgba , 3 , item ) ;
PyTuple_SetItem ( visualShapeObList , 7 , rgba ) ;
}
2016-10-19 05:05:28 +00:00
PyTuple_SetItem ( pyResultList , i , visualShapeObList ) ;
}
return pyResultList ;
}
else
{
PyErr_SetString ( SpamError , " Error receiving visual shape info " ) ;
return NULL ;
}
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_resetVisualShapeData ( PyObject * self , PyObject * args , PyObject * keywds )
2016-10-22 00:48:06 +00:00
{
2017-03-13 23:06:52 +00:00
int objectUniqueId = - 1 ;
int jointIndex = - 1 ;
int shapeIndex = - 1 ;
int textureUniqueId = - 1 ;
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
int physicsClientId = 0 ;
2016-12-28 04:25:52 +00:00
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " objectUniqueId " , " jointIndex " , " shapeIndex " , " textureUniqueId " , " physicsClientId " , NULL } ;
2016-12-28 04:25:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iiii|i " , kwlist , & objectUniqueId , & jointIndex , & shapeIndex , & textureUniqueId , & physicsClientId ) )
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
2016-11-10 05:01:04 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
{
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 ;
}
}
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-10-22 00:48:06 +00:00
}
2016-10-19 05:05:28 +00:00
2016-12-28 04:25:52 +00:00
static PyObject * pybullet_loadTexture ( PyObject * self , PyObject * args , PyObject * keywds )
2016-10-22 00:48:06 +00:00
{
2017-03-13 23:06:52 +00:00
const char * filename = 0 ;
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " textureFilename " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " s|i " , kwlist , & filename , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
2016-11-10 05:01:04 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-10-19 05:05:28 +00:00
2017-03-13 23:06:52 +00:00
{
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 ;
}
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2016-09-02 01:28:39 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * MyConvertContactPoint ( struct b3ContactInformation * contactPointPtr )
2016-11-10 05:01:04 +00:00
{
2017-03-13 23:06:52 +00:00
/*
2016-09-08 18:12:58 +00:00
0 int m_contactFlags ;
1 int m_bodyUniqueIdA ;
2 int m_bodyUniqueIdB ;
3 int m_linkIndexA ;
4 int m_linkIndexB ;
2016-11-20 01:13:56 +00:00
5 double m_positionOnAInWS [ 3 ] ; //contact point location on object A,
2016-09-08 18:12:58 +00:00
in world space coordinates
2016-11-20 01:13:56 +00:00
6 double m_positionOnBInWS [ 3 ] ; //contact point location on object
2016-09-08 18:12:58 +00:00
A , in world space coordinates
2016-11-20 01:13:56 +00:00
7 double m_contactNormalOnBInWS [ 3 ] ; //the separating contact
2016-09-08 18:12:58 +00:00
normal , pointing from object B towards object A
2016-11-20 01:13:56 +00:00
8 double m_contactDistance ; //negative number is penetration, positive
2016-09-08 18:12:58 +00:00
is distance .
2016-11-20 01:13:56 +00:00
9 double m_normalForce ;
2016-09-08 18:12:58 +00:00
*/
2016-11-10 05:01:04 +00:00
int i ;
PyObject * pyResultList = PyTuple_New ( contactPointPtr - > m_numContactPoints ) ;
2017-03-13 23:06:52 +00:00
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 ;
2016-11-10 05:01:04 +00:00
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getOverlappingObjects ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-10 05:01:04 +00:00
{
2017-03-13 23:06:52 +00:00
PyObject * aabbMinOb = 0 , * aabbMaxOb = 0 ;
2016-11-10 05:01:04 +00:00
double aabbMin [ 3 ] ;
double aabbMax [ 3 ] ;
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
struct b3AABBOverlapData overlapData ;
2016-11-10 19:22:22 +00:00
int i ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " aabbMin " , " aabbMax " , " physicsClientId " , NULL } ;
2016-11-10 19:22:22 +00:00
2016-12-28 04:25:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " OO|i " , kwlist ,
2017-03-13 23:06:52 +00:00
& aabbMinOb , & aabbMaxOb , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
return NULL ;
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
2016-11-10 05:01:04 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
pybullet_internalSetVectord ( aabbMinOb , aabbMin ) ;
pybullet_internalSetVectord ( aabbMaxOb , aabbMax ) ;
commandHandle = b3InitAABBOverlapQuery ( sm , aabbMin , aabbMax ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
b3GetAABBOverlapResults ( sm , & overlapData ) ;
2016-11-10 19:22:22 +00:00
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!
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < overlapData . m_numOverlappingObjects ; i + + )
{
PyObject * overlap = PyTuple_New ( 2 ) ; //body unique id and link index
2016-11-10 19:22:22 +00:00
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 ;
}
2016-11-10 05:01:04 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getClosestPointData ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-10 05:01:04 +00:00
{
2017-03-13 23:06:52 +00:00
int bodyUniqueIdA = - 1 ;
int bodyUniqueIdB = - 1 ;
int linkIndexA = - 2 ;
int linkIndexB = - 2 ;
2016-11-20 01:13:56 +00:00
2017-03-13 23:06:52 +00:00
double distanceThreshold = 0.f ;
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
b3SharedMemoryCommandHandle commandHandle ;
struct b3ContactInformation contactPointData ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " bodyA " , " bodyB " , " distance " , " linkIndexA " , " linkIndexB " , " physicsClientId " , NULL } ;
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iid|iii " , kwlist ,
& bodyUniqueIdA , & bodyUniqueIdB , & distanceThreshold , & linkIndexA , & linkIndexB , & physicsClientId ) )
return NULL ;
2016-11-10 05:01:04 +00:00
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
commandHandle = b3InitClosestDistanceQuery ( sm ) ;
b3SetClosestDistanceFilterBodyA ( commandHandle , bodyUniqueIdA ) ;
b3SetClosestDistanceFilterBodyB ( commandHandle , bodyUniqueIdB ) ;
b3SetClosestDistanceThreshold ( commandHandle , distanceThreshold ) ;
if ( linkIndexA > = - 1 )
{
b3SetClosestDistanceFilterLinkA ( commandHandle , linkIndexA ) ;
}
if ( linkIndexB > = - 1 )
{
b3SetClosestDistanceFilterLinkB ( commandHandle , linkIndexB ) ;
}
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
if ( statusType = = CMD_CONTACT_POINT_INFORMATION_COMPLETED )
{
b3GetContactPointInformation ( sm , & contactPointData ) ;
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
return MyConvertContactPoint ( & contactPointData ) ;
}
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-11-10 05:01:04 +00:00
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_changeUserConstraint ( PyObject * self , PyObject * args , PyObject * keywds )
2017-01-12 18:30:46 +00:00
{
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " userConstraintUniqueId " , " jointChildPivot " , " jointChildFrameOrientation " , " maxForce " , " physicsClientId " , NULL } ;
int userConstraintUniqueId = - 1 ;
2017-01-12 18:30:46 +00:00
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
PyObject * jointChildPivotObj = 0 ;
PyObject * jointChildFrameOrnObj = 0 ;
2017-01-12 18:30:46 +00:00
double jointChildPivot [ 3 ] ;
double jointChildFrameOrn [ 4 ] ;
2017-01-17 02:17:18 +00:00
double maxForce = - 1 ;
2017-01-12 18:30:46 +00:00
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|OOdi " , kwlist , & userConstraintUniqueId , & jointChildPivotObj , & jointChildFrameOrnObj , & maxForce , & physicsClientId ) )
2017-01-12 18:30:46 +00:00
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2017-01-12 18:30:46 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
commandHandle = b3InitChangeUserConstraintCommand ( sm , userConstraintUniqueId ) ;
2017-01-12 18:30:46 +00:00
2017-03-13 23:06:52 +00:00
if ( pybullet_internalSetVectord ( jointChildPivotObj , jointChildPivot ) )
2017-01-12 18:30:46 +00:00
{
b3InitChangeUserConstraintSetPivotInB ( commandHandle , jointChildPivot ) ;
}
2017-03-13 23:06:52 +00:00
if ( pybullet_internalSetVector4d ( jointChildFrameOrnObj , jointChildFrameOrn ) )
2017-01-12 18:30:46 +00:00
{
b3InitChangeUserConstraintSetFrameInB ( commandHandle , jointChildFrameOrn ) ;
}
2017-03-13 23:06:52 +00:00
if ( maxForce > = 0 )
2017-01-17 02:17:18 +00:00
{
2017-03-13 23:06:52 +00:00
b3InitChangeUserConstraintSetMaxForce ( commandHandle , maxForce ) ;
2017-01-17 02:17:18 +00:00
}
2017-01-12 18:30:46 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
Py_INCREF ( Py_None ) ;
return Py_None ;
} ;
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_removeUserConstraint ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-14 22:08:05 +00:00
{
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " userConstraintUniqueId " , " physicsClientId " , NULL } ;
int userConstraintUniqueId = - 1 ;
2016-11-14 22:08:05 +00:00
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " i|i " , kwlist , & userConstraintUniqueId , & physicsClientId ) )
2016-11-14 22:08:05 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-11-14 22:08:05 +00:00
{
2016-12-28 04:25:52 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
2016-11-14 22:08:05 +00:00
return NULL ;
}
2017-03-13 23:06:52 +00:00
commandHandle = b3InitRemoveUserConstraintCommand ( sm , userConstraintUniqueId ) ;
2016-11-14 22:08:05 +00:00
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 ;
}
*/
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_enableJointForceTorqueSensor ( PyObject * self , PyObject * args , PyObject * keywds )
2017-01-31 18:14:00 +00:00
{
int bodyUniqueId = - 1 ;
int jointIndex = - 1 ;
int enableSensor = 1 ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
int numJoints = - 1 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " bodyUniqueId " , " jointIndex " , " enableSensor " , " physicsClientId " } ;
2017-01-31 18:14:00 +00:00
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ii|ii " , kwlist , & bodyUniqueId , & jointIndex , & enableSensor , & physicsClientId ) )
2017-01-31 18:14:00 +00:00
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
if ( bodyUniqueId < 0 )
{
PyErr_SetString ( SpamError , " Error: invalid bodyUniqueId " ) ;
return NULL ;
}
numJoints = b3GetNumJoints ( sm , bodyUniqueId ) ;
if ( ( jointIndex < 0 ) | | ( jointIndex > = numJoints ) )
{
PyErr_SetString ( SpamError , " Error: invalid jointIndex. " ) ;
return NULL ;
}
{
b3SharedMemoryCommandHandle commandHandle ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
commandHandle = b3CreateSensorCommandInit ( sm , bodyUniqueId ) ;
b3CreateSensorEnable6DofJointForceTorqueSensor ( commandHandle , jointIndex , enableSensor ) ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
if ( statusType = = CMD_CLIENT_COMMAND_COMPLETED )
{
Py_INCREF ( Py_None ) ;
return Py_None ;
}
}
PyErr_SetString ( SpamError , " Error creating sensor. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_createUserConstraint ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-14 22:08:05 +00:00
{
b3SharedMemoryCommandHandle commandHandle ;
2017-03-13 23:06:52 +00:00
int parentBodyUniqueId = - 1 ;
int parentLinkIndex = - 1 ;
int childBodyUniqueId = - 1 ;
int childLinkIndex = - 1 ;
int jointType = ePoint2PointType ;
PyObject * jointAxisObj = 0 ;
double jointAxis [ 3 ] = { 0 , 0 , 0 } ;
2016-11-14 22:08:05 +00:00
PyObject * parentFramePositionObj = 0 ;
2017-03-13 23:06:52 +00:00
double parentFramePosition [ 3 ] = { 0 , 0 , 0 } ;
2016-11-14 22:08:05 +00:00
PyObject * childFramePositionObj = 0 ;
2017-03-13 23:06:52 +00:00
double childFramePosition [ 3 ] = { 0 , 0 , 0 } ;
2016-11-14 22:08:05 +00:00
PyObject * parentFrameOrientationObj = 0 ;
2017-03-13 23:06:52 +00:00
double parentFrameOrientation [ 4 ] = { 0 , 0 , 0 , 1 } ;
2016-11-14 22:08:05 +00:00
PyObject * childFrameOrientationObj = 0 ;
2017-03-13 23:06:52 +00:00
double childFrameOrientation [ 4 ] = { 0 , 0 , 0 , 1 } ;
2016-11-14 22:08:05 +00:00
struct b3JointInfo jointInfo ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " parentBodyUniqueId " , " parentLinkIndex " ,
" childBodyUniqueId " , " childLinkIndex " ,
" jointType " ,
" jointAxis " ,
" parentFramePosition " ,
" childFramePosition " ,
" parentFrameOrientation " ,
" childFrameOrientation " ,
" physicsClientId " ,
NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iiiiiOOO|OOi " , kwlist , & parentBodyUniqueId , & parentLinkIndex ,
& childBodyUniqueId , & childLinkIndex ,
& jointType , & jointAxisObj ,
& parentFramePositionObj ,
& childFramePositionObj ,
& parentFrameOrientationObj ,
& childFrameOrientationObj ,
& physicsClientId ) )
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
pybullet_internalSetVectord ( jointAxisObj , jointAxis ) ;
pybullet_internalSetVectord ( parentFramePositionObj , parentFramePosition ) ;
pybullet_internalSetVectord ( childFramePositionObj , childFramePosition ) ;
pybullet_internalSetVector4d ( parentFrameOrientationObj , parentFrameOrientation ) ;
pybullet_internalSetVector4d ( childFrameOrientationObj , childFrameOrientation ) ;
2016-11-14 22:08:05 +00:00
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 ] ;
2017-03-13 23:06:52 +00:00
2016-11-14 22:08:05 +00:00
jointInfo . m_jointAxis [ 0 ] = jointAxis [ 0 ] ;
jointInfo . m_jointAxis [ 1 ] = jointAxis [ 1 ] ;
jointInfo . m_jointAxis [ 2 ] = jointAxis [ 2 ] ;
2017-03-13 23:06:52 +00:00
commandHandle = b3InitCreateUserConstraintCommand ( sm , parentBodyUniqueId , parentLinkIndex , childBodyUniqueId , childLinkIndex , & jointInfo ) ;
2016-11-14 22:08:05 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
2017-03-13 23:06:52 +00:00
if ( statusType = = CMD_USER_CONSTRAINT_COMPLETED )
2016-11-14 22:08:05 +00:00
{
int userConstraintUid = b3GetStatusUserConstraintUniqueId ( statusHandle ) ;
PyObject * ob = PyLong_FromLong ( userConstraintUid ) ;
return ob ;
}
2017-01-31 18:14:00 +00:00
PyErr_SetString ( SpamError , " createConstraint failed. " ) ;
return NULL ;
2016-11-14 22:08:05 +00:00
}
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getContactPointData ( PyObject * self , PyObject * args , PyObject * keywds )
{
int bodyUniqueIdA = - 1 ;
int bodyUniqueIdB = - 1 ;
2017-03-29 19:04:23 +00:00
int linkIndexA = - 2 ;
int linkIndexB = - 2 ;
2017-03-13 23:06:52 +00:00
b3SharedMemoryCommandHandle commandHandle ;
struct b3ContactInformation contactPointData ;
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
2016-11-10 05:01:04 +00:00
2017-03-29 19:04:23 +00:00
static char * kwlist [ ] = { " bodyA " , " bodyB " , " linkIndexA " , " linkIndexB " , " physicsClientId " , NULL } ;
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
2017-03-29 19:04:23 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " |iiiii " , kwlist ,
& bodyUniqueIdA , & bodyUniqueIdB , & linkIndexA , & linkIndexB , & physicsClientId ) )
2017-03-13 23:06:52 +00:00
return NULL ;
2016-11-10 05:01:04 +00:00
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
commandHandle = b3InitRequestContactPointInformation ( sm ) ;
2017-03-29 19:04:23 +00:00
if ( bodyUniqueIdA > = 0 )
{
b3SetContactFilterBodyA ( commandHandle , bodyUniqueIdA ) ;
}
if ( bodyUniqueIdB > = 0 )
{
b3SetContactFilterBodyB ( commandHandle , bodyUniqueIdB ) ;
}
if ( linkIndexA > = - 1 )
{
b3SetContactFilterLinkA ( commandHandle , linkIndexA ) ;
}
if ( linkIndexB > = - 1 )
{
b3SetContactFilterLinkB ( commandHandle , linkIndexB ) ;
}
2017-03-13 23:06:52 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , commandHandle ) ;
statusType = b3GetStatusType ( statusHandle ) ;
if ( statusType = = CMD_CONTACT_POINT_INFORMATION_COMPLETED )
{
b3GetContactPointInformation ( sm , & contactPointData ) ;
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
return MyConvertContactPoint ( & contactPointData ) ;
}
2016-09-02 01:28:39 +00:00
2017-03-13 23:06:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-09-08 18:12:58 +00:00
}
2016-09-02 01:28:39 +00:00
2016-11-17 23:15:52 +00:00
/// Render an image from the current timestep of the simulation, width, height are required, other args are optional
2017-03-04 21:31:30 +00:00
// getCameraImage(w, h, view[16], projection[16], lightDir[3], lightColor[3], lightDist, hasShadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff, renderer)
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getCameraImage ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-17 23:15:52 +00:00
{
2017-03-04 21:31:30 +00:00
/// request an image from a simulated camera, using software or hardware renderer.
2016-11-17 23:15:52 +00:00
struct b3CameraImageData imageData ;
2017-03-13 23:06:52 +00:00
PyObject * objViewMat = 0 , * objProjMat = 0 , * lightDirObj = 0 , * lightColorObj = 0 ;
2016-11-17 23:15:52 +00:00
int width , height ;
float viewMatrix [ 16 ] ;
float projectionMatrix [ 16 ] ;
float lightDir [ 3 ] ;
2017-03-13 23:06:52 +00:00
float lightColor [ 3 ] ;
float lightDist = 10.0 ;
int hasShadow = 0 ;
float lightAmbientCoeff = 0.6 ;
float lightDiffuseCoeff = 0.35 ;
float lightSpecularCoeff = 0.05 ;
int renderer = 0 ;
2016-11-17 23:15:52 +00:00
// inialize cmd
b3SharedMemoryCommandHandle command ;
2016-12-28 04:25:52 +00:00
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
// set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " width " , " height " , " viewMatrix " , " projectionMatrix " , " lightDirection " , " lightColor " , " lightDistance " , " shadow " , " lightAmbientCoeff " , " lightDiffuseCoeff " , " lightSpecularCoeff " , " renderer " , " physicsClientId " , NULL } ;
2016-11-17 23:15:52 +00:00
2017-03-04 21:31:30 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ii|OOOOfifffii " , kwlist , & width , & height , & objViewMat , & objProjMat , & lightDirObj , & lightColorObj , & lightDist , & hasShadow , & lightAmbientCoeff , & lightDiffuseCoeff , & lightSpecularCoeff , & renderer , & physicsClientId ) )
2016-11-17 23:15:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-11-17 23:15:52 +00:00
{
2016-12-28 04:25:52 +00:00
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
2016-11-17 23:15:52 +00:00
return NULL ;
}
2016-12-28 04:25:52 +00:00
command = b3InitRequestCameraImage ( sm ) ;
2016-11-17 23:15:52 +00:00
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 ) ;
}
2016-11-20 21:14:18 +00:00
//set light direction only if function succeeds
2016-11-17 23:15:52 +00:00
if ( pybullet_internalSetVector ( lightDirObj , lightDir ) )
{
b3RequestCameraImageSetLightDirection ( command , lightDir ) ;
}
2017-03-13 23:06:52 +00:00
//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 ) ;
2017-03-27 15:30:20 +00:00
b3RequestCameraImageSelectRenderer ( command , renderer ) ; //renderer could be ER_BULLET_HARDWARE_OPENGL
2016-11-17 23:15:52 +00:00
if ( b3CanSubmitCommand ( sm ) )
{
b3SharedMemoryStatusHandle statusHandle ;
int statusType ;
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
statusType = b3GetStatusType ( statusHandle ) ;
2017-03-13 23:06:52 +00:00
if ( statusType = = CMD_CAMERA_IMAGE_COMPLETED )
{
2016-11-17 23:15:52 +00:00
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
2017-03-13 23:06:52 +00:00
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 } ;
2016-11-17 23:15:52 +00:00
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 ,
2017-03-13 23:06:52 +00:00
imageData . m_pixelHeight * imageData . m_pixelWidth * bytesPerPixel ) ;
2016-11-17 23:15:52 +00:00
memcpy ( PyArray_DATA ( pyDep ) , imageData . m_depthValues ,
2017-03-13 23:06:52 +00:00
imageData . m_pixelHeight * imageData . m_pixelWidth ) ;
2016-11-17 23:15:52 +00:00
memcpy ( PyArray_DATA ( pySeg ) , imageData . m_segmentationMaskValues ,
2017-03-13 23:06:52 +00:00
imageData . m_pixelHeight * imageData . m_pixelWidth ) ;
2016-11-17 23:15:52 +00:00
PyTuple_SetItem ( pyResultList , 2 , pyRGB ) ;
PyTuple_SetItem ( pyResultList , 3 , pyDep ) ;
PyTuple_SetItem ( pyResultList , 4 , pySeg ) ;
2017-03-13 23:06:52 +00:00
# else //PYBULLET_USE_NUMPY
2016-11-17 23:15:52 +00:00
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 ) ;
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < imageData . m_pixelWidth ; i + + )
{
for ( j = 0 ; j < imageData . m_pixelHeight ; j + + )
{
2016-11-17 23:15:52 +00:00
// 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 ) ;
}
2017-03-13 23:06:52 +00:00
for ( p = 0 ; p < bytesPerPixel ; p + + )
{
2016-11-17 23:15:52 +00:00
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 ;
2017-03-13 23:06:52 +00:00
# endif //PYBULLET_USE_NUMPY
2016-11-17 23:15:52 +00:00
return pyResultList ;
}
}
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_computeViewMatrix ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-17 23:15:52 +00:00
{
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
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " cameraEyePosition " , " cameraTargetPosition " , " cameraUpVector " , NULL } ;
2016-11-17 23:15:52 +00:00
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 ] ;
2017-03-13 23:06:52 +00:00
PyObject * pyResultList = 0 ;
2016-11-17 23:15:52 +00:00
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
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_computeViewMatrixFromYawPitchRoll ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-17 23:15:52 +00:00
{
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
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " cameraTargetPosition " , " distance " , " yaw " , " pitch " , " roll " , " upAxisIndex " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " Offffi " , kwlist , & cameraTargetPositionObj , & distance , & yaw , & pitch , & roll , & upAxisIndex ) )
2016-11-17 23:15:52 +00:00
{
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
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_computeProjectionMatrix ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-17 23:15:52 +00:00
{
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
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " left " , " right " , " bottom " , " top " , " nearVal " , " farVal " , NULL } ;
2016-11-17 23:15:52 +00:00
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 ;
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_computeProjectionMatrixFOV ( PyObject * self , PyObject * args , PyObject * keywds )
2016-11-17 23:15:52 +00:00
{
2017-03-13 23:06:52 +00:00
float fov , aspect , nearVal , farVal ;
2016-11-17 23:15:52 +00:00
PyObject * pyResultList = 0 ;
float projectionMatrix [ 16 ] ;
int i ;
2017-03-13 23:06:52 +00:00
static char * kwlist [ ] = { " fov " , " aspect " , " nearVal " , " farVal " , NULL } ;
2016-11-17 23:15:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " ffff " , kwlist , & fov , & aspect , & nearVal , & farVal ) )
{
return NULL ;
}
2017-03-13 23:06:52 +00:00
2016-11-17 23:15:52 +00:00
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 ) ;
}
2017-03-13 23:06:52 +00:00
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 ;
b3PhysicsClientHandle sm ;
int physicsClientId = 0 ;
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
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 ;
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 , PyObject * keywds )
{
{
int objectUniqueId = - 1 , linkIndex = - 1 , flags ;
double force [ 3 ] ;
double position [ 3 ] = { 0.0 , 0.0 , 0.0 } ;
PyObject * forceObj = 0 , * posObj = 0 ;
b3SharedMemoryCommandHandle command ;
b3SharedMemoryStatusHandle statusHandle ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " objectUniqueId " , " linkIndex " ,
" forceObj " , " posObj " , " flags " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iiOOi|i " , kwlist , & objectUniqueId , & linkIndex ,
& forceObj , & posObj , & flags , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
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 ;
2016-11-17 23:15:52 +00:00
}
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_applyExternalTorque ( PyObject * self , PyObject * args , PyObject * keywds )
{
{
int objectUniqueId , linkIndex , flags ;
double torque [ 3 ] ;
PyObject * torqueObj ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " objectUniqueId " , " linkIndex " , " torqueObj " ,
" flags " , " physicsClientId " , NULL } ;
2016-11-17 23:15:52 +00:00
2017-03-13 23:06:52 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iiOi|i " , kwlist , & objectUniqueId , & linkIndex , & torqueObj ,
& flags , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
2016-08-02 18:12:23 +00:00
2017-03-13 23:06:52 +00:00
{
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 ) ;
2016-06-24 22:30:43 +00:00
2017-03-13 23:06:52 +00:00
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 ) ;
}
}
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2016-06-24 22:30:43 +00:00
2017-03-13 23:06:52 +00:00
static PyObject * pybullet_getQuaternionFromEuler ( PyObject * self ,
PyObject * args )
{
double rpy [ 3 ] ;
2016-06-10 22:07:41 +00:00
2017-03-13 23:06:52 +00:00
PyObject * eulerObj ;
2016-12-29 05:51:54 +00:00
2017-03-13 23:06:52 +00:00
if ( PyArg_ParseTuple ( args , " O " , & eulerObj ) )
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
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 ) ;
2016-12-28 04:25:52 +00:00
}
2017-03-13 23:06:52 +00:00
else
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError ,
" Euler angles need a 3 coordinates [roll, pitch, yaw]. " ) ;
2016-12-28 04:25:52 +00:00
return NULL ;
}
2017-03-13 23:06:52 +00:00
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
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 ;
}
}
2016-12-28 04:25:52 +00:00
}
2017-03-13 23:06:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-06-27 01:18:30 +00:00
}
2017-03-13 23:06:52 +00:00
/// 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 ;
2016-06-27 01:18:30 +00:00
2017-03-13 23:06:52 +00:00
double quat [ 4 ] ;
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
PyObject * quatObj ;
if ( PyArg_ParseTuple ( args , " O " , & quatObj ) )
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
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 ) ;
2016-12-28 04:25:52 +00:00
}
2017-03-13 23:06:52 +00:00
else
2016-12-28 04:25:52 +00:00
{
2017-03-13 23:06:52 +00:00
PyErr_SetString ( SpamError , " Quaternion need a 4 components [x,y,z,w]. " ) ;
2016-12-28 04:25:52 +00:00
return NULL ;
}
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
{
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 ;
2016-08-10 01:40:12 +00:00
}
2016-12-15 05:11:53 +00:00
///Inverse Kinematics binding
static PyObject * pybullet_calculateInverseKinematics ( PyObject * self ,
2017-03-13 23:06:52 +00:00
PyObject * args , PyObject * keywds )
2016-12-28 04:25:52 +00:00
2017-03-13 23:06:52 +00:00
{
int bodyIndex ;
int endEffectorLinkIndex ;
2016-09-22 20:27:09 +00:00
2017-03-13 23:06:52 +00:00
PyObject * targetPosObj = 0 ;
PyObject * targetOrnObj = 0 ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
PyObject * lowerLimitsObj = 0 ;
PyObject * upperLimitsObj = 0 ;
PyObject * jointRangesObj = 0 ;
PyObject * restPosesObj = 0 ;
PyObject * jointDampingObj = 0 ;
2017-01-12 05:39:22 +00:00
2017-05-12 16:38:17 +00:00
static char * kwlist [ ] = { " bodyIndex " , " endEffectorLinkIndex " , " targetPosition " , " targetOrientation " , " lowerLimits " , " upperLimits " , " jointRanges " , " restPoses " , " jointDamping " , " physicsClientId " , NULL } ;
2017-05-12 18:18:33 +00:00
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iiO|OOOOOOi " , kwlist , & bodyIndex , & endEffectorLinkIndex , & targetPosObj , & targetOrnObj , & lowerLimitsObj , & upperLimitsObj , & jointRangesObj , & restPosesObj , & jointDampingObj , & physicsClientId ) )
2016-12-28 04:25:52 +00:00
{
return NULL ;
}
2016-12-29 05:51:54 +00:00
sm = getPhysicsClient ( physicsClientId ) ;
2017-03-13 23:06:52 +00:00
if ( sm = = 0 )
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
double pos [ 3 ] ;
2017-03-13 23:06:52 +00:00
double ori [ 4 ] = { 0 , 1.0 , 0 , 0 } ;
int hasPos = pybullet_internalSetVectord ( targetPosObj , pos ) ;
int hasOrn = pybullet_internalSetVector4d ( targetOrnObj , ori ) ;
2017-01-12 05:39:22 +00:00
int szLowerLimits = lowerLimitsObj ? PySequence_Size ( lowerLimitsObj ) : 0 ;
2017-03-13 23:06:52 +00:00
int szUpperLimits = upperLimitsObj ? PySequence_Size ( upperLimitsObj ) : 0 ;
int szJointRanges = jointRangesObj ? PySequence_Size ( jointRangesObj ) : 0 ;
int szRestPoses = restPosesObj ? PySequence_Size ( restPosesObj ) : 0 ;
int szJointDamping = jointDampingObj ? PySequence_Size ( jointDampingObj ) : 0 ;
2017-01-12 05:39:22 +00:00
int numJoints = b3GetNumJoints ( sm , bodyIndex ) ;
int hasNullSpace = 0 ;
2017-03-13 23:06:52 +00:00
int hasJointDamping = 0 ;
2017-01-12 05:39:22 +00:00
double * lowerLimits = 0 ;
double * upperLimits = 0 ;
double * jointRanges = 0 ;
double * restPoses = 0 ;
2017-03-13 23:06:52 +00:00
double * jointDamping = 0 ;
2017-01-12 05:39:22 +00:00
if ( numJoints & & ( szLowerLimits = = numJoints ) & & ( szUpperLimits = = numJoints ) & &
2017-03-13 23:06:52 +00:00
( szJointRanges = = numJoints ) & & ( szRestPoses = = numJoints ) )
2017-01-12 05:39:22 +00:00
{
int szInBytes = sizeof ( double ) * numJoints ;
int i ;
lowerLimits = ( double * ) malloc ( szInBytes ) ;
upperLimits = ( double * ) malloc ( szInBytes ) ;
jointRanges = ( double * ) malloc ( szInBytes ) ;
restPoses = ( double * ) malloc ( szInBytes ) ;
2017-03-13 23:06:52 +00:00
for ( i = 0 ; i < numJoints ; i + + )
2017-01-12 05:39:22 +00:00
{
2017-03-13 23:06:52 +00:00
lowerLimits [ i ] =
pybullet_internalGetFloatFromSequence ( lowerLimitsObj , i ) ;
upperLimits [ i ] =
pybullet_internalGetFloatFromSequence ( upperLimitsObj , i ) ;
jointRanges [ i ] =
pybullet_internalGetFloatFromSequence ( jointRangesObj , i ) ;
restPoses [ i ] =
pybullet_internalGetFloatFromSequence ( restPosesObj , i ) ;
2017-01-12 05:39:22 +00:00
}
hasNullSpace = 1 ;
}
2017-03-13 23:06:52 +00:00
if ( numJoints & & ( szJointDamping = = numJoints ) )
{
int szInBytes = sizeof ( double ) * numJoints ;
int i ;
jointDamping = ( double * ) malloc ( szInBytes ) ;
for ( i = 0 ; i < numJoints ; i + + )
{
jointDamping [ i ] = pybullet_internalGetFloatFromSequence ( jointDampingObj , i ) ;
}
hasJointDamping = 1 ;
}
2017-01-12 05:39:22 +00:00
if ( hasPos )
2016-12-28 04:25:52 +00:00
{
b3SharedMemoryStatusHandle statusHandle ;
2017-03-13 23:06:52 +00:00
int numPos = 0 ;
2016-12-28 04:25:52 +00:00
int resultBodyIndex ;
int result ;
2017-03-13 23:06:52 +00:00
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit ( sm , bodyIndex ) ;
2017-01-12 05:39:22 +00:00
if ( hasNullSpace )
{
if ( hasOrn )
{
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel ( command , numJoints , endEffectorLinkIndex , pos , ori , lowerLimits , upperLimits , jointRanges , restPoses ) ;
2017-03-13 23:06:52 +00:00
}
else
2017-01-12 05:39:22 +00:00
{
2017-03-13 23:06:52 +00:00
b3CalculateInverseKinematicsPosWithNullSpaceVel ( command , numJoints , endEffectorLinkIndex , pos , lowerLimits , upperLimits , jointRanges , restPoses ) ;
2017-01-12 05:39:22 +00:00
}
2017-03-13 23:06:52 +00:00
}
else
2017-01-12 05:39:22 +00:00
{
if ( hasOrn )
{
2017-03-13 23:06:52 +00:00
b3CalculateInverseKinematicsAddTargetPositionWithOrientation ( command , endEffectorLinkIndex , pos , ori ) ;
}
else
2017-01-12 05:39:22 +00:00
{
2017-03-13 23:06:52 +00:00
b3CalculateInverseKinematicsAddTargetPurePosition ( command , endEffectorLinkIndex , pos ) ;
2017-01-12 05:39:22 +00:00
}
}
2017-03-13 23:06:52 +00:00
if ( hasJointDamping )
{
b3CalculateInverseKinematicsSetJointDamping ( command , numJoints , jointDamping ) ;
}
2017-01-12 05:39:22 +00:00
2016-12-28 04:25:52 +00:00
statusHandle = b3SubmitClientCommandAndWaitStatus ( sm , command ) ;
2017-03-13 23:06:52 +00:00
2016-12-28 04:25:52 +00:00
result = b3GetStatusInverseKinematicsJointPositions ( statusHandle ,
2017-03-13 23:06:52 +00:00
& resultBodyIndex ,
& numPos ,
0 ) ;
2016-12-28 04:25:52 +00:00
if ( result & & numPos )
{
int i ;
PyObject * pylist ;
2017-03-13 23:06:52 +00:00
double * ikOutPutJointPos = ( double * ) malloc ( numPos * sizeof ( double ) ) ;
2016-12-28 04:25:52 +00:00
result = b3GetStatusInverseKinematicsJointPositions ( statusHandle ,
& resultBodyIndex ,
& numPos ,
ikOutPutJointPos ) ;
pylist = PyTuple_New ( numPos ) ;
for ( i = 0 ; i < numPos ; i + + )
{
PyTuple_SetItem ( pylist , i ,
PyFloat_FromDouble ( ikOutPutJointPos [ i ] ) ) ;
}
2017-03-13 23:06:52 +00:00
2016-12-28 04:25:52 +00:00
free ( ikOutPutJointPos ) ;
return pylist ;
}
else
{
PyErr_SetString ( SpamError ,
" Error in calculateInverseKinematics " ) ;
return NULL ;
}
2017-03-13 23:06:52 +00:00
}
else
2016-12-28 04:25:52 +00:00
{
PyErr_SetString ( SpamError ,
2017-03-13 23:06:52 +00:00
" calculateInverseKinematics couldn't extract position vector3 " ) ;
2016-12-28 04:25:52 +00:00
return NULL ;
}
}
2016-09-13 22:37:46 +00:00
2017-03-13 23:06:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
}
2016-09-13 22:37:46 +00:00
2016-09-08 18:12:58 +00:00
/// Given an object id, joint positions, joint velocities and joint
/// accelerations,
/// compute the joint forces using Inverse Dynamics
static PyObject * pybullet_calculateInverseDynamics ( PyObject * self ,
2017-03-13 23:06:52 +00:00
PyObject * args , PyObject * keywds )
2016-12-28 04:25:52 +00:00
{
{
2017-03-13 23:06:52 +00:00
int bodyIndex ;
PyObject * objPositionsQ ;
PyObject * objVelocitiesQdot ;
PyObject * objAccelerations ;
int physicsClientId = 0 ;
b3PhysicsClientHandle sm = 0 ;
static char * kwlist [ ] = { " bodyIndex " , " objPositions " , " objVelocities " , " objAccelerations " , " physicsClientId " , NULL } ;
if ( ! PyArg_ParseTupleAndKeywords ( args , keywds , " iOOO|i " , kwlist , & bodyIndex , & objPositionsQ ,
& objVelocitiesQdot , & objAccelerations , & physicsClientId ) )
{
return NULL ;
}
sm = getPhysicsClient ( physicsClientId ) ;
if ( sm = = 0 )
{
PyErr_SetString ( SpamError , " Not connected to physics server. " ) ;
return NULL ;
}
{
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 ;
}
}
2016-12-28 04:25:52 +00:00
}
2017-03-13 23:06:52 +00:00
Py_INCREF ( Py_None ) ;
return Py_None ;
2016-09-08 18:12:58 +00:00
}
2016-06-10 22:07:41 +00:00
2016-04-30 18:18:54 +00:00
static PyMethodDef SpamMethods [ ] = {
2016-06-10 22:07:41 +00:00
2017-03-13 23:06:52 +00:00
{ " connect " , ( PyCFunction ) pybullet_connectPhysicsServer , METH_VARARGS | METH_KEYWORDS ,
" Connect to an existing physics server (using shared memory by default). " } ,
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
{ " disconnect " , ( PyCFunction ) pybullet_disconnectPhysicsServer , METH_VARARGS | METH_KEYWORDS ,
" Disconnect from the physics server. " } ,
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
{ " resetSimulation " , ( PyCFunction ) pybullet_resetSimulation , METH_VARARGS | METH_KEYWORDS ,
" Reset the simulation: remove all objects and start from an empty world. " } ,
2016-06-10 22:07:41 +00:00
2017-03-13 23:06:52 +00:00
{ " stepSimulation " , ( PyCFunction ) pybullet_stepSimulation , METH_VARARGS | METH_KEYWORDS ,
" Step the simulation using forward dynamics. " } ,
2016-05-03 19:59:21 +00:00
2017-03-13 23:06:52 +00:00
{ " setGravity " , ( PyCFunction ) pybullet_setGravity , METH_VARARGS | METH_KEYWORDS ,
" Set the gravity acceleration (x,y,z). " } ,
2016-05-10 07:57:54 +00:00
2017-03-13 23:06:52 +00:00
{ " setTimeStep " , ( PyCFunction ) pybullet_setTimeStep , METH_VARARGS | METH_KEYWORDS ,
" Set the amount of time to proceed at each call to stepSimulation. (unit "
" is seconds, typically range is 0.01 or 0.001) " } ,
2016-05-10 07:57:54 +00:00
2017-03-13 23:06:52 +00:00
{ " setDefaultContactERP " , ( PyCFunction ) pybullet_setDefaultContactERP , METH_VARARGS | METH_KEYWORDS ,
" 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. " } ,
2016-12-01 06:24:20 +00:00
2017-03-13 23:06:52 +00:00
{ " setRealTimeSimulation " , ( PyCFunction ) pybullet_setRealTimeSimulation , METH_VARARGS | METH_KEYWORDS ,
" Enable or disable real time simulation (using the real time clock, "
" RTC) in the physics server. Expects one integer argument, 0 or 1 " } ,
2016-12-01 06:24:20 +00:00
2017-03-13 23:06:52 +00:00
{ " setPhysicsEngineParameter " , ( PyCFunction ) pybullet_setPhysicsEngineParameter , METH_VARARGS | METH_KEYWORDS ,
" Set some internal physics engine parameter, such as cfm or erp etc. " } ,
2016-07-18 06:50:11 +00:00
2017-03-13 23:06:52 +00:00
{ " setInternalSimFlags " , ( PyCFunction ) pybullet_setInternalSimFlags , METH_VARARGS | METH_KEYWORDS ,
" This is for experimental purposes, use at own risk, magic may or not happen " } ,
2016-12-01 06:24:20 +00:00
2017-03-13 23:06:52 +00:00
{ " loadURDF " , ( PyCFunction ) pybullet_loadURDF , METH_VARARGS | METH_KEYWORDS ,
" Create a multibody by loading a URDF file. " } ,
2016-06-15 21:21:04 +00:00
2017-03-13 23:06:52 +00:00
{ " loadSDF " , ( PyCFunction ) pybullet_loadSDF , METH_VARARGS | METH_KEYWORDS ,
" Load multibodies from an SDF file. " } ,
2016-06-24 14:31:17 +00:00
2017-03-13 23:06:52 +00:00
{ " loadBullet " , ( PyCFunction ) pybullet_loadBullet , METH_VARARGS | METH_KEYWORDS ,
" Restore the full state of the world from a .bullet file. " } ,
2016-11-11 22:44:50 +00:00
2017-03-13 23:06:52 +00:00
{ " saveBullet " , ( PyCFunction ) pybullet_saveBullet , METH_VARARGS | METH_KEYWORDS ,
" Save the full state of the world to a .bullet file. " } ,
2016-11-11 22:44:50 +00:00
2017-03-13 23:06:52 +00:00
{ " loadMJCF " , ( PyCFunction ) pybullet_loadMJCF , METH_VARARGS | METH_KEYWORDS ,
" Load multibodies from an MJCF file. " } ,
2016-11-11 22:44:50 +00:00
2017-03-13 23:06:52 +00:00
{ " createConstraint " , ( PyCFunction ) pybullet_createUserConstraint , METH_VARARGS | METH_KEYWORDS ,
" Create a constraint between two bodies. Returns a (int) unique id, if successfull. " } ,
2016-11-14 22:08:05 +00:00
2017-01-12 18:30:46 +00:00
{ " changeConstraint " , ( PyCFunction ) pybullet_changeUserConstraint , METH_VARARGS | METH_KEYWORDS ,
2017-03-13 23:06:52 +00:00
" Change some parameters of an existing constraint, such as the child pivot or child frame orientation, using its unique id. " } ,
2017-01-12 18:30:46 +00:00
2016-11-14 22:08:05 +00:00
{ " removeConstraint " , ( PyCFunction ) pybullet_removeUserConstraint , METH_VARARGS | METH_KEYWORDS ,
2017-03-13 23:06:52 +00:00
" Remove a constraint using its unique id. " } ,
{ " enableJointForceTorqueSensor " , ( PyCFunction ) pybullet_enableJointForceTorqueSensor , METH_VARARGS | METH_KEYWORDS ,
" Enable or disable a joint force/torque sensor measuring the joint reaction forces. " } ,
{ " saveWorld " , ( PyCFunction ) pybullet_saveWorld , METH_VARARGS | METH_KEYWORDS ,
" Save a approximate Python file to reproduce the current state of the world: saveWorld "
" (filename). (very preliminary and approximately) " } ,
{ " getNumBodies " , ( PyCFunction ) pybullet_getNumBodies , METH_VARARGS | METH_KEYWORDS ,
" Get the number of bodies in the simulation. " } ,
{ " getBodyUniqueId " , ( PyCFunction ) pybullet_getBodyUniqueId , METH_VARARGS | METH_KEYWORDS ,
" getBodyUniqueId is used after connecting to server with existing bodies. "
2017-01-23 03:08:31 +00:00
" Get the unique id of the body, given a integer range [0.. number of bodies). " } ,
2016-09-27 19:13:45 +00:00
2017-03-13 23:06:52 +00:00
{ " getBodyInfo " , ( PyCFunction ) pybullet_getBodyInfo , METH_VARARGS | METH_KEYWORDS ,
" Get the body info, given a body unique id. " } ,
2017-05-04 04:53:29 +00:00
{ " removeBody " , ( PyCFunction ) pybullet_removeBody , METH_VARARGS | METH_KEYWORDS ,
" Remove a body by its body unique id. " } ,
2017-03-13 23:06:52 +00:00
{ " getNumConstraints " , ( PyCFunction ) pybullet_getNumConstraints , METH_VARARGS | METH_KEYWORDS ,
" Get the number of user-created constraints in the simulation. " } ,
{ " getConstraintInfo " , ( PyCFunction ) pybullet_getConstraintInfo , METH_VARARGS | METH_KEYWORDS ,
" Get the user-created constraint info, given a constraint unique id. " } ,
2017-05-04 01:25:25 +00:00
{ " getConstraintUniqueId " , ( PyCFunction ) pybullet_getConstraintUniqueId , METH_VARARGS | METH_KEYWORDS ,
2017-03-13 23:06:52 +00:00
" Get the unique id of the constraint, given a integer index in range [0.. number of constraints). " } ,
{ " getBasePositionAndOrientation " , ( PyCFunction ) pybullet_getBasePositionAndOrientation ,
METH_VARARGS | METH_KEYWORDS ,
" 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 " ,
( PyCFunction ) pybullet_resetBasePositionAndOrientation , METH_VARARGS | METH_KEYWORDS ,
" 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 " , ( PyCFunction ) pybullet_getBaseVelocity ,
METH_VARARGS | METH_KEYWORDS ,
" 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 " , ( PyCFunction ) pybullet_getNumJoints , METH_VARARGS | METH_KEYWORDS ,
" Get the number of joints for an object. " } ,
{ " getJointInfo " , ( PyCFunction ) pybullet_getJointInfo , METH_VARARGS | METH_KEYWORDS ,
" Get the name and type info for a joint on a body. " } ,
{ " getJointState " , ( PyCFunction ) pybullet_getJointState , METH_VARARGS | METH_KEYWORDS ,
" Get the state (position, velocity etc) for a joint on a body. " } ,
2017-03-29 22:37:33 +00:00
{ " getLinkState " , ( PyCFunction ) pybullet_getLinkState , METH_VARARGS | METH_KEYWORDS ,
2017-03-13 23:06:52 +00:00
" Provides extra information such as the Cartesian world coordinates "
" center of mass (COM) of the link, relative to the world reference "
" frame. " } ,
{ " resetJointState " , ( PyCFunction ) pybullet_resetJointState , METH_VARARGS | METH_KEYWORDS ,
" Reset the state (position, velocity etc) for a joint on a body "
" instantaneously, not through physics simulation. " } ,
2017-05-02 05:18:54 +00:00
2017-05-09 17:44:33 +00:00
{ " changeDynamicsInfo " , ( PyCFunction ) pybullet_changeDynamicsInfo , METH_VARARGS | METH_KEYWORDS ,
" change dynamics information such as mass, lateral friction coefficient. " } ,
2017-05-08 05:41:05 +00:00
2017-05-09 17:31:28 +00:00
{ " getDynamicsInfo " , ( PyCFunction ) pybullet_getDynamicsInfo , METH_VARARGS | METH_KEYWORDS ,
" Get dynamics information such as mass, lateral friction coefficient. " } ,
2017-03-13 23:06:52 +00:00
{ " setJointMotorControl " , ( PyCFunction ) pybullet_setJointMotorControl , METH_VARARGS ,
" This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead. "
" Set a single joint motor control mode and desired target value. There is "
" no immediate state change, stepSimulation will process the motors. " } ,
{ " setJointMotorControl2 " , ( PyCFunction ) pybullet_setJointMotorControl2 , METH_VARARGS | METH_KEYWORDS ,
2016-12-28 04:25:52 +00:00
" Set a single joint motor control mode and desired target value. There is "
2017-03-13 23:06:52 +00:00
" no immediate state change, stepSimulation will process the motors. " } ,
2017-05-08 20:41:07 +00:00
{ " setJointMotorControlArray " , ( PyCFunction ) pybullet_setJointMotorControlArray , METH_VARARGS | METH_KEYWORDS ,
" Set an array of motors control mode and desired target value. There is "
" no immediate state change, stepSimulation will process the motors. "
" This is similar to setJointMotorControl2, with jointIndices as a list, and optional targetPositions, "
" targetVelocities, forces, kds and kps as lists "
" Using setJointMotorControlArray has the benefit of lower calling overhead. "
} ,
2017-03-13 23:06:52 +00:00
{ " applyExternalForce " , ( PyCFunction ) pybullet_applyExternalForce , METH_VARARGS | METH_KEYWORDS ,
" 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 "
" WORLD_FRAME coordinates " } ,
{ " applyExternalTorque " , ( PyCFunction ) pybullet_applyExternalTorque , METH_VARARGS | METH_KEYWORDS ,
" 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 "
" 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, lightSpecularCoeff, and renderer), and return the "
" 8-8-8bit RGB pixel data and floating point depth values "
2016-09-11 10:35:12 +00:00
# ifdef PYBULLET_USE_NUMPY
2017-03-13 23:06:52 +00:00
" as NumPy arrays "
2016-09-11 10:35:12 +00:00
# endif
2017-03-13 23:06:52 +00:00
} ,
2016-11-17 23:15:52 +00:00
2017-03-13 23:06:52 +00:00
{ " computeViewMatrix " , ( PyCFunction ) pybullet_computeViewMatrix , METH_VARARGS | METH_KEYWORDS ,
" Compute a camera viewmatrix from camera eye, target position and up vector " } ,
2016-11-17 23:15:52 +00:00
2017-03-13 23:06:52 +00:00
{ " computeViewMatrixFromYawPitchRoll " , ( PyCFunction ) pybullet_computeViewMatrixFromYawPitchRoll , METH_VARARGS | METH_KEYWORDS ,
" Compute a camera viewmatrix from camera eye, target position and up vector " } ,
2016-11-17 23:15:52 +00:00
2017-03-13 23:06:52 +00:00
{ " computeProjectionMatrix " , ( PyCFunction ) pybullet_computeProjectionMatrix , METH_VARARGS | METH_KEYWORDS ,
" Compute a camera projection matrix from screen left/right/bottom/top/near/far values " } ,
2016-06-27 01:18:30 +00:00
2017-03-13 23:06:52 +00:00
{ " computeProjectionMatrixFOV " , ( PyCFunction ) pybullet_computeProjectionMatrixFOV , METH_VARARGS | METH_KEYWORDS ,
" Compute a camera projection matrix from fov, aspect ratio, near, far values " } ,
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
{ " 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. " } ,
2016-11-10 05:01:04 +00:00
2017-03-13 23:06:52 +00:00
{ " 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. " } ,
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
{ " 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]. " } ,
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
{ " addUserDebugLine " , ( PyCFunction ) pybullet_addUserDebugLine , METH_VARARGS | METH_KEYWORDS ,
2016-11-14 15:39:34 +00:00
" Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. "
2017-03-13 23:06:52 +00:00
" A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item. " } ,
2016-11-14 15:39:34 +00:00
2017-03-13 23:06:52 +00:00
{ " addUserDebugText " , ( PyCFunction ) pybullet_addUserDebugText , METH_VARARGS | METH_KEYWORDS ,
2016-11-14 15:39:34 +00:00
" Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds "
2017-03-13 23:06:52 +00:00
" A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item. " } ,
{ " addUserDebugParameter " , ( PyCFunction ) pybullet_addUserDebugParameter , METH_VARARGS | METH_KEYWORDS ,
" Add a user debug parameter, such as a slider, that can be controlled using a GUI. " } ,
{ " readUserDebugParameter " , ( PyCFunction ) pybullet_readUserDebugParameter , METH_VARARGS | METH_KEYWORDS ,
" Read the current value of a user debug parameter, given the user debug item unique id. " } ,
{ " 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. " } ,
2017-04-10 18:03:41 +00:00
{ " getDebugVisualizerCamera " , ( PyCFunction ) pybullet_getDebugVisualizerCamera , METH_VARARGS | METH_KEYWORDS ,
" Get information about the 3D visualizer camera, such as width, height, view matrix, projection matrix etc. " } ,
2017-03-13 23:06:52 +00:00
{ " configureDebugVisualizer " , ( PyCFunction ) pybullet_configureDebugVisualizer , METH_VARARGS | METH_KEYWORDS ,
" For the 3D OpenGL Visualizer, enable/disable GUI, shadows. " } ,
{ " resetDebugVisualizerCamera " , ( PyCFunction ) pybullet_resetDebugVisualizerCamera , METH_VARARGS | METH_KEYWORDS ,
" For the 3D OpenGL Visualizer, set the camera distance, yaw, pitch and target position. " } ,
{ " getVisualShapeData " , ( PyCFunction ) pybullet_getVisualShapeData , METH_VARARGS | METH_KEYWORDS ,
" Return the visual shape information for one object. " } ,
{ " resetVisualShapeData " , ( PyCFunction ) pybullet_resetVisualShapeData , METH_VARARGS | METH_KEYWORDS ,
" Reset part of the visual shape information for one object. " } ,
{ " loadTexture " , ( PyCFunction ) pybullet_loadTexture , METH_VARARGS | METH_KEYWORDS ,
" 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 " } ,
{ " getMatrixFromQuaternion " , pybullet_getMatrixFromQuaternion , METH_VARARGS ,
" Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major) " } ,
{ " calculateInverseDynamics " , ( PyCFunction ) pybullet_calculateInverseDynamics , METH_VARARGS | METH_KEYWORDS ,
" Given an object id, joint positions, joint velocities and joint "
" accelerations, compute the joint forces using Inverse Dynamics " } ,
{ " calculateInverseKinematics " , ( PyCFunction ) pybullet_calculateInverseKinematics ,
METH_VARARGS | METH_KEYWORDS ,
" Inverse Kinematics bindings: Given an object id, "
" current joint positions and target position "
" for the end effector, "
" compute the inverse kinematics and return the new joint state " } ,
{ " getVREvents " , ( PyCFunction ) pybullet_getVREvents , METH_VARARGS | METH_KEYWORDS ,
" Get Virtual Reality events, for example to track VR controllers position/buttons " } ,
{ " setVRCameraState " , ( PyCFunction ) pybullet_setVRCameraState , METH_VARARGS | METH_KEYWORDS ,
" Set properties of the VR Camera such as its root transform "
" for teleporting or to track objects (camera inside a vehicle for example). " } ,
{ " getKeyboardEvents " , ( PyCFunction ) pybullet_getKeyboardEvents , METH_VARARGS | METH_KEYWORDS ,
" Get Keyboard events, keycode and state (KEY_IS_DOWN, KEY_WAS_TRIGGER, KEY_WAS_RELEASED) " } ,
{ " startStateLogging " , ( PyCFunction ) pybullet_startStateLogging , METH_VARARGS | METH_KEYWORDS ,
" Start logging of state, such as robot base position, orientation, joint positions etc. "
2017-04-02 23:03:20 +00:00
" Specify loggingType (STATE_LOGGING_MINITAUR, STATE_LOGGING_GENERIC_ROBOT, STATE_LOGGING_VR_CONTROLLERS, STATE_LOGGING_CONTACT_POINTS, etc), "
2017-04-04 20:45:49 +00:00
" fileName, optional objectUniqueId, maxLogDof, bodyUniqueIdA, bodyUniqueIdB, linkIndexA, linkIndexB. Function returns int loggingUniqueId " } ,
2017-03-13 23:06:52 +00:00
{ " stopStateLogging " , ( PyCFunction ) pybullet_stopStateLogging , METH_VARARGS | METH_KEYWORDS ,
" Stop logging of robot state, given a loggingUniqueId. " } ,
2017-04-05 22:21:26 +00:00
{ " rayTest " , ( PyCFunction ) pybullet_rayTestObsolete , METH_VARARGS | METH_KEYWORDS ,
2017-03-13 23:06:52 +00:00
" Cast a ray and return the first object hit, if any. "
2017-04-05 22:21:26 +00:00
" Takes two arguments (from_position [x,y,z] and to_position [x,y,z] in Cartesian world coordinates " } ,
{ " rayTestBatch " , ( PyCFunction ) pybullet_rayTestBatch , METH_VARARGS | METH_KEYWORDS ,
" Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
" Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates " } ,
2017-05-05 00:51:40 +00:00
{ " submitProfileTiming " , ( PyCFunction ) pybullet_submitProfileTiming , METH_VARARGS | METH_KEYWORDS ,
" Add a custom profile timing that will be visible in performance profile recordings on the physics server. "
" On the physics server (in GUI and VR mode) you can press 'p' to start and/or stop profile recordings " } ,
2017-03-13 23:06:52 +00:00
{ " setTimeOut " , ( PyCFunction ) pybullet_setTimeOut , METH_VARARGS | METH_KEYWORDS ,
" Set the timeOut in seconds, used for most of the API calls. " } ,
// todo(erwincoumans)
// saveSnapshot
// loadSnapshot
// raycast info
// object names
{ NULL , NULL , 0 , NULL } /* Sentinel */
2016-04-30 18:18:54 +00:00
} ;
2017-03-13 23:06:52 +00:00
///copied from CommonWindowInterface.h
2017-03-02 20:33:22 +00:00
2017-03-13 23:06:52 +00:00
enum
{
2017-03-02 20:33:22 +00:00
B3G_ESCAPE = 27 ,
B3G_F1 = 0xff00 ,
B3G_F2 ,
B3G_F3 ,
B3G_F4 ,
B3G_F5 ,
B3G_F6 ,
B3G_F7 ,
B3G_F8 ,
B3G_F9 ,
B3G_F10 ,
B3G_F11 ,
B3G_F12 ,
B3G_F13 ,
B3G_F14 ,
B3G_F15 ,
B3G_LEFT_ARROW ,
B3G_RIGHT_ARROW ,
B3G_UP_ARROW ,
B3G_DOWN_ARROW ,
B3G_PAGE_UP ,
B3G_PAGE_DOWN ,
B3G_END ,
B3G_HOME ,
B3G_INSERT ,
B3G_DELETE ,
B3G_BACKSPACE ,
B3G_SHIFT ,
B3G_CONTROL ,
B3G_ALT ,
B3G_RETURN
} ;
2016-05-10 07:57:54 +00:00
# if PY_MAJOR_VERSION >= 3
2016-09-08 18:12:58 +00:00
static struct PyModuleDef moduledef = {
2017-03-13 23:06:52 +00:00
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 */
2016-09-08 18:12:58 +00:00
} ;
2016-05-10 07:57:54 +00:00
# endif
2016-04-30 18:18:54 +00:00
PyMODINIT_FUNC
2016-05-10 07:57:54 +00:00
# if PY_MAJOR_VERSION >= 3
PyInit_pybullet ( void )
# else
2016-04-30 18:18:54 +00:00
initpybullet ( void )
2016-05-10 07:57:54 +00:00
# endif
2016-04-30 18:18:54 +00:00
{
2017-03-13 23:06:52 +00:00
PyObject * m ;
2016-05-10 07:57:54 +00:00
# if PY_MAJOR_VERSION >= 3
2017-03-13 23:06:52 +00:00
m = PyModule_Create ( & moduledef ) ;
2016-05-10 07:57:54 +00:00
# else
2017-03-13 23:06:52 +00:00
m = Py_InitModule3 ( " pybullet " , SpamMethods , " Python bindings for Bullet " ) ;
2016-05-10 07:57:54 +00:00
# endif
2016-05-18 22:07:42 +00:00
# if PY_MAJOR_VERSION >= 3
2017-03-13 23:06:52 +00:00
if ( m = = NULL ) return m ;
2016-05-18 22:07:42 +00:00
# else
2017-03-13 23:06:52 +00:00
if ( m = = NULL ) return ;
2016-05-18 22:07:42 +00:00
# endif
2016-09-08 18:12:58 +00:00
2017-03-13 23:06:52 +00:00
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 , " TCP " , eCONNECT_TCP ) ; // user read
PyModule_AddIntConstant ( m , " JOINT_REVOLUTE " , eRevoluteType ) ; // user read
PyModule_AddIntConstant ( m , " JOINT_PRISMATIC " , ePrismaticType ) ; // user read
PyModule_AddIntConstant ( m , " JOINT_SPHERICAL " , eSphericalType ) ; // user read
PyModule_AddIntConstant ( m , " JOINT_PLANAR " , ePlanarType ) ; // user read
PyModule_AddIntConstant ( m , " JOINT_FIXED " , eFixedType ) ; // user read
PyModule_AddIntConstant ( m , " JOINT_POINT2POINT " , ePoint2PointType ) ; // user read
PyModule_AddIntConstant ( m , " SENSOR_FORCE_TORQUE " , eSensorForceTorqueType ) ; // 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 ) ;
PyModule_AddIntConstant ( m , " VR_BUTTON_IS_DOWN " , eButtonIsDown ) ;
PyModule_AddIntConstant ( m , " VR_BUTTON_WAS_TRIGGERED " , eButtonTriggered ) ;
PyModule_AddIntConstant ( m , " VR_BUTTON_WAS_RELEASED " , eButtonReleased ) ;
PyModule_AddIntConstant ( m , " VR_MAX_CONTROLLERS " , MAX_VR_CONTROLLERS ) ;
PyModule_AddIntConstant ( m , " VR_MAX_BUTTONS " , MAX_VR_BUTTONS ) ;
2017-04-08 05:53:36 +00:00
PyModule_AddIntConstant ( m , " VR_DEVICE_CONTROLLER " , VR_DEVICE_CONTROLLER ) ;
PyModule_AddIntConstant ( m , " VR_DEVICE_HMD " , VR_DEVICE_HMD ) ;
PyModule_AddIntConstant ( m , " VR_DEVICE_GENERIC_TRACKER " , VR_DEVICE_GENERIC_TRACKER ) ;
2017-03-13 23:06:52 +00:00
PyModule_AddIntConstant ( m , " KEY_IS_DOWN " , eButtonIsDown ) ;
PyModule_AddIntConstant ( m , " KEY_WAS_TRIGGERED " , eButtonTriggered ) ;
PyModule_AddIntConstant ( m , " KEY_WAS_RELEASED " , eButtonReleased ) ;
PyModule_AddIntConstant ( m , " STATE_LOGGING_MINITAUR " , STATE_LOGGING_MINITAUR ) ;
PyModule_AddIntConstant ( m , " STATE_LOGGING_GENERIC_ROBOT " , STATE_LOGGING_GENERIC_ROBOT ) ;
PyModule_AddIntConstant ( m , " STATE_LOGGING_VR_CONTROLLERS " , STATE_LOGGING_VR_CONTROLLERS ) ;
2017-03-22 03:43:23 +00:00
PyModule_AddIntConstant ( m , " STATE_LOGGING_VIDEO_MP4 " , STATE_LOGGING_VIDEO_MP4 ) ;
2017-04-02 23:03:20 +00:00
PyModule_AddIntConstant ( m , " STATE_LOGGING_CONTACT_POINTS " , STATE_LOGGING_CONTACT_POINTS ) ;
2017-05-05 00:51:40 +00:00
PyModule_AddIntConstant ( m , " STATE_LOGGING_PROFILE_TIMINGS " , STATE_LOGGING_PROFILE_TIMINGS ) ;
2017-03-13 23:06:52 +00:00
PyModule_AddIntConstant ( m , " COV_ENABLE_GUI " , COV_ENABLE_GUI ) ;
PyModule_AddIntConstant ( m , " COV_ENABLE_SHADOWS " , COV_ENABLE_SHADOWS ) ;
PyModule_AddIntConstant ( m , " COV_ENABLE_WIREFRAME " , COV_ENABLE_WIREFRAME ) ;
PyModule_AddIntConstant ( m , " ER_TINY_RENDERER " , ER_TINY_RENDERER ) ;
PyModule_AddIntConstant ( m , " ER_BULLET_HARDWARE_OPENGL " , ER_BULLET_HARDWARE_OPENGL ) ;
2017-03-27 15:30:20 +00:00
PyModule_AddIntConstant ( m , " URDF_USE_INERTIA_FROM_FILE " , URDF_USE_INERTIA_FROM_FILE ) ;
2017-04-04 19:47:34 +00:00
PyModule_AddIntConstant ( m , " URDF_USE_SELF_COLLISION " , URDF_USE_SELF_COLLISION ) ;
2017-05-10 22:01:25 +00:00
PyModule_AddIntConstant ( m , " URDF_USE_SELF_COLLISION_EXCLUDE_PARENT " , URDF_USE_SELF_COLLISION_EXCLUDE_PARENT ) ;
PyModule_AddIntConstant ( m , " URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS " , URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS ) ;
2017-04-05 22:21:26 +00:00
PyModule_AddIntConstant ( m , " MAX_RAY_INTERSECTION_BATCH_SIZE " , MAX_RAY_INTERSECTION_BATCH_SIZE ) ;
2017-03-13 23:06:52 +00:00
PyModule_AddIntConstant ( m , " B3G_F1 " , B3G_F1 ) ;
PyModule_AddIntConstant ( m , " B3G_F2 " , B3G_F2 ) ;
PyModule_AddIntConstant ( m , " B3G_F3 " , B3G_F3 ) ;
PyModule_AddIntConstant ( m , " B3G_F4 " , B3G_F4 ) ;
PyModule_AddIntConstant ( m , " B3G_F5 " , B3G_F5 ) ;
PyModule_AddIntConstant ( m , " B3G_F6 " , B3G_F6 ) ;
PyModule_AddIntConstant ( m , " B3G_F7 " , B3G_F7 ) ;
PyModule_AddIntConstant ( m , " B3G_F8 " , B3G_F8 ) ;
PyModule_AddIntConstant ( m , " B3G_F9 " , B3G_F9 ) ;
PyModule_AddIntConstant ( m , " B3G_F10 " , B3G_F10 ) ;
PyModule_AddIntConstant ( m , " B3G_F11 " , B3G_F11 ) ;
PyModule_AddIntConstant ( m , " B3G_F12 " , B3G_F12 ) ;
PyModule_AddIntConstant ( m , " B3G_F13 " , B3G_F13 ) ;
PyModule_AddIntConstant ( m , " B3G_F14 " , B3G_F14 ) ;
PyModule_AddIntConstant ( m , " B3G_F15 " , B3G_F15 ) ;
PyModule_AddIntConstant ( m , " B3G_LEFT_ARROW " , B3G_LEFT_ARROW ) ;
PyModule_AddIntConstant ( m , " B3G_RIGHT_ARROW " , B3G_RIGHT_ARROW ) ;
PyModule_AddIntConstant ( m , " B3G_UP_ARROW " , B3G_UP_ARROW ) ;
PyModule_AddIntConstant ( m , " B3G_DOWN_ARROW " , B3G_DOWN_ARROW ) ;
PyModule_AddIntConstant ( m , " B3G_PAGE_UP " , B3G_PAGE_UP ) ;
PyModule_AddIntConstant ( m , " B3G_PAGE_DOWN " , B3G_PAGE_DOWN ) ;
PyModule_AddIntConstant ( m , " B3G_END " , B3G_END ) ;
PyModule_AddIntConstant ( m , " B3G_HOME " , B3G_HOME ) ;
PyModule_AddIntConstant ( m , " B3G_INSERT " , B3G_INSERT ) ;
PyModule_AddIntConstant ( m , " B3G_DELETE " , B3G_DELETE ) ;
PyModule_AddIntConstant ( m , " B3G_BACKSPACE " , B3G_BACKSPACE ) ;
PyModule_AddIntConstant ( m , " B3G_SHIFT " , B3G_SHIFT ) ;
PyModule_AddIntConstant ( m , " B3G_CONTROL " , B3G_CONTROL ) ;
PyModule_AddIntConstant ( m , " B3G_ALT " , B3G_ALT ) ;
PyModule_AddIntConstant ( m , " B3G_RETURN " , B3G_RETURN ) ;
SpamError = PyErr_NewException ( " pybullet.error " , NULL , NULL ) ;
Py_INCREF ( SpamError ) ;
PyModule_AddObject ( m , " error " , SpamError ) ;
2016-09-11 10:09:51 +00:00
2017-03-30 20:50:02 +00:00
Py_AtExit ( b3pybulletExitFunc ) ;
2016-09-11 10:09:51 +00:00
# ifdef PYBULLET_USE_NUMPY
2017-03-13 23:06:52 +00:00
// Initialize numpy array.
import_array ( ) ;
# endif //PYBULLET_USE_NUMPY
2016-09-11 10:09:51 +00:00
2016-05-10 07:57:54 +00:00
# if PY_MAJOR_VERSION >= 3
2017-03-13 23:06:52 +00:00
return m ;
2016-05-10 07:57:54 +00:00
# endif
2016-04-30 18:18:54 +00:00
}