diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b735caeb1..17c092b2d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -121,7 +121,7 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) // Load a URDF file indicating the links and joints of an object // function can be called without arguments and will default // to position (0,0,1) with orientation(0,0,0,1) -// else, loadURDF(x,y,z) or +// els(x,y,z) or // loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w) static PyObject * pybullet_loadURDF(PyObject* self, PyObject* args) @@ -290,6 +290,7 @@ pybullet_setGravity(PyObject* self, PyObject* args) } + // Internal function used to get the base position and orientation // Orientation is returned in quaternions static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3]) @@ -305,7 +306,6 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double { - { b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(sm, bodyIndex); @@ -315,12 +315,18 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double const int status_type = b3GetStatusType(status_handle); const double* actualStateQ; + // const double* jointReactionForces[]; + int i; 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]; @@ -331,6 +337,7 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double baseOrientation[1] = actualStateQ[4]; baseOrientation[2] = actualStateQ[5]; baseOrientation[3] = actualStateQ[6]; + } } } @@ -359,7 +366,7 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "Expected a body index (integer)."); return NULL; } - + pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation); { @@ -430,26 +437,340 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) } } -// TODO(hellojas): set joint positions for a body +// Initalize all joint positions given a list of values static PyObject* -pybullet_setJointPositions(PyObject* self, PyObject* args) +pybullet_initializeJointPositions(PyObject* self, PyObject* args) { if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - + // TODO(hellojas): initialize all joint positions given a pylist of values + Py_INCREF(Py_None); return Py_None; } -// const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes -// const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats + +// CURRENTLY NOT SUPPORTED +// Initalize a single joint position for a specific body index +// +// This method skips any physics simulation and +// teleports all joints to the new positions. + +// TODO(hellojas): initializing one joint currently not supported +// static PyObject* +// pybullet_initializeJointPosition(PyObject* self, PyObject* args) +// { +// if (0==sm) +// { +// PyErr_SetString(SpamError, "Not connected to physics server."); +// return NULL; +// } +// +// int i; +// int bodyIndex = -1; +// int jointIndex = -1; +// double jointPos = 0.0; +// +// int size= PySequence_Size(args); +// +// if (size==3) // get body index, joint index, and joint position value +// { +// if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &jointPos)) +// { +// b3SharedMemoryCommandHandle cmd_handle = b3CreatePoseCommandInit(sm, bodyIndex); +// +// // printf("initializing joint %d at %f\n", jointIndex, jointPos); +// b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos); +// +// b3SharedMemoryStatusHandle status_handle = +// b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); +// +// const int status_type = b3GetStatusType(status_handle); +// +// } +// } +// +// Py_INCREF(Py_None); +// return Py_None; +// } + +static void pybullet_internalGetJointPositions(int bodyIndex, int numJoints, double jointPositions[]) { + int i, j; + int numDegreeQ; + int numDegreeU; + int arrSizeOfPosAndOrn = 7; + + for (i =0;i