From eeaff0747b47c46b1eb86e239475475e684762d8 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Tue, 14 Jun 2016 14:08:24 -0700 Subject: [PATCH 1/5] return joint state given a joint index and body index --- examples/pybullet/pybullet.c | 182 ++++++++++++++++++++++++++++++++++- 1 file changed, 179 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f056b5b57..e4ffeeedf 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -235,6 +235,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]) @@ -260,12 +261,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]; @@ -300,6 +307,7 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) return NULL; } + double basePosition[3]; double baseOrientation[4]; @@ -375,14 +383,173 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) // TODO(hellojas): set joint positions for a body 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; } - + + + Py_INCREF(Py_None); + return Py_None; +} + + +// TODO(hellojas): get joint positions for a body +static PyObject* +pybullet_getJointInfo(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; +} + + +// Returns the state of a specific joint in a given bodyIndex +// The state of a joint includes the following: +// position, velocity, force torque (6 values), and motor torque +// The returned pylist is an array of [float, float, float[6], float] + +// TODO(hellojas): check accuracy of position and velocity +// TODO(hellojas): check force torque values + +static PyObject* +pybullet_getJointState(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + PyObject *pyListJointForceTorque; + PyObject *pyListJointState; + PyObject *item; + + struct b3JointInfo info; + struct b3JointSensorState sensorState; + + int bodyIndex = -1; + int jointIndex = -1; + int sensorStateSize = 4; // size of struct b3JointSensorState + int forceTorqueSize = 6; // size of force torque list from b3JointSensorState + int i, j; + + + int size= PySequence_Size(args); + + if (size==2) // get body index and joint index + { + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) + { + + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + pyListJointState = PyTuple_New(sensorStateSize); + pyListJointForceTorque = PyTuple_New(forceTorqueSize); + + + // double m_jointPosition; + // double m_jointVelocity; + // double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ + // double m_jointMotorTorque; + b3GetJointState(sm, status_handle, jointIndex, &sensorState); + // printf("Joint%d: position=%f velocity=%f motortorque=%f\n", + // jointIndex, + // sensorState.m_jointPosition, + // sensorState.m_jointVelocity, + // sensorState.m_jointMotorTorque); + + PyTuple_SetItem(pyListJointState, 0, + PyFloat_FromDouble(sensorState.m_jointPosition)); + PyTuple_SetItem(pyListJointState, 1, + PyFloat_FromDouble(sensorState.m_jointVelocity)); + + // joint force torque is list of 6 + /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ + // printf(" jointForceTorque = "); + for (j = 0; j < forceTorqueSize; j++) { + // printf("%f ", sensorState.m_jointForceTorque[j]); + PyTuple_SetItem(pyListJointForceTorque, j, + PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); + } + + PyTuple_SetItem(pyListJointState, 2, + pyListJointForceTorque); + + PyTuple_SetItem(pyListJointState, 3, + PyFloat_FromDouble(sensorState.m_jointMotorTorque)); + + return pyListJointState; + } + } + + Py_INCREF(Py_None); + return Py_None; +} + + +// TODO(hellojas): get joint positions for a body +static PyObject* +pybullet_getJointPositionAndVelocity(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + struct b3JointInfo info; + int bodyIndex = 0; + int forceTorqueSize = 6; + PyObject *pylistJointPos; + PyObject *item; + int i, j, numJoints; + struct b3JointSensorState sensorState; + + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + numJoints = b3GetNumJoints(sm,bodyIndex); + + const char *pyListJointNames[numJoints]; + pylistJointPos = PyTuple_New(numJoints); + + +// double m_jointPosition; +// double m_jointVelocity; +// double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ +// double m_jointMotorTorque; + for (i = 0; i < numJoints; ++i) { + b3GetJointState(sm, status_handle, i, &sensorState); + printf("Joint%d: position=%f velocity=%f motortorque=%f\n", + i, + sensorState.m_jointPosition, + sensorState.m_jointVelocity, + sensorState.m_jointMotorTorque); + + // item = PyFloat_FromDouble(basePosition[i]); + + printf(" jointForceTorque = "); + for (j = 0; j < forceTorqueSize; j++) { + printf("%f ", sensorState.m_jointPosition); + } + printf("\n"); +} + Py_INCREF(Py_None); return Py_None; } @@ -565,9 +732,18 @@ static PyMethodDef SpamMethods[] = { {"setGravity", pybullet_setGravity, METH_VARARGS, "Set the gravity acceleration (x,y,z)."}, - {"initializeJointPositions", pybullet_setJointPositions, METH_VARARGS, + {"initializeJointPositions", pybullet_initializeJointPositions, METH_VARARGS, "Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."}, + // {"getJointPositions", pybullet_getJointPositions, METH_VARARGS, + // "Get the joint positions for all joints."}, + // + {"getJointInfo", pybullet_getJointInfo, METH_VARARGS, + "Get the joint metadata info for a joint on a body. This includes joint index, name, type, q-index and u-index."}, + + {"getJointState", pybullet_getJointState, METH_VARARGS, + "Get the joint metadata info for a joint on a body."}, + {"renderImage", pybullet_renderImage, METH_VARARGS, "Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"}, From d53d6366deebfbd223e0075009e9d4d7dce4a911 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Tue, 14 Jun 2016 14:35:45 -0700 Subject: [PATCH 2/5] get the state of a specific joint given a body index --- examples/pybullet/pybullet.c | 59 +++++++++++++++++++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8cd657d81..afcdcb76d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -306,7 +306,6 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double { - { b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(sm, bodyIndex); @@ -463,6 +462,64 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) return NULL; } + PyObject *pyListJointInfo; + + struct b3JointInfo info; + + int bodyIndex = -1; + int jointIndex = -1; + int jointInfoSize = 8; //size of struct b3JointInfo + + int size= PySequence_Size(args); + + if (size==2) // get body index and joint index + { + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) + { + + // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); + + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + pyListJointInfo = PyTuple_New(jointInfoSize); + + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + + PyTuple_SetItem(pyListJointInfo, 0, + PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, + PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, + PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, + PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, + PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + + return pyListJointInfo; + } + } + Py_INCREF(Py_None); return Py_None; } From d6ab1ab43416df3207d5d9f48231cd9548b5ae9f Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Tue, 14 Jun 2016 14:58:36 -0700 Subject: [PATCH 3/5] initialize a single joint position for a given body index --- examples/pybullet/pybullet.c | 119 +++++++++++++++++------------------ 1 file changed, 57 insertions(+), 62 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index afcdcb76d..b645045b3 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -436,7 +436,7 @@ 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_initializeJointPositions(PyObject* self, PyObject* args) { @@ -445,13 +445,65 @@ pybullet_initializeJointPositions(PyObject* self, PyObject* args) 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; } +// 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): test accuracy of joint position values +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; +} + + +// Get the a single joint info for a specific bodyIndex +// +// Joint information includes: +// index, name, type, q-index, u-index, +// flags, joint damping, joint friction +// +// The format of the returned list is +// [int, str, int, int, int, int, float, float] +// // TODO(hellojas): get joint positions for a body static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) @@ -613,60 +665,6 @@ pybullet_getJointState(PyObject* self, PyObject* args) } -// TODO(hellojas): get joint positions for a body -static PyObject* -pybullet_getJointPositionAndVelocity(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - struct b3JointInfo info; - int bodyIndex = 0; - int forceTorqueSize = 6; - PyObject *pylistJointPos; - PyObject *item; - int i, j, numJoints; - struct b3JointSensorState sensorState; - - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - - numJoints = b3GetNumJoints(sm,bodyIndex); - - const char *pyListJointNames[numJoints]; - pylistJointPos = PyTuple_New(numJoints); - - -// double m_jointPosition; -// double m_jointVelocity; -// double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ -// double m_jointMotorTorque; - for (i = 0; i < numJoints; ++i) { - b3GetJointState(sm, status_handle, i, &sensorState); - printf("Joint%d: position=%f velocity=%f motortorque=%f\n", - i, - sensorState.m_jointPosition, - sensorState.m_jointVelocity, - sensorState.m_jointMotorTorque); - - // item = PyFloat_FromDouble(basePosition[i]); - - printf(" jointForceTorque = "); - for (j = 0; j < forceTorqueSize; j++) { - printf("%f ", sensorState.m_jointPosition); - } - printf("\n"); -} - - 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 @@ -852,9 +850,9 @@ static PyMethodDef SpamMethods[] = { {"initializeJointPositions", pybullet_initializeJointPositions, METH_VARARGS, "Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."}, - // {"getJointPositions", pybullet_getJointPositions, METH_VARARGS, - // "Get the joint positions for all joints."}, - // + {"initializeJointPosition", pybullet_initializeJointPosition, METH_VARARGS, + "Initialize the joint position for one joint. This method skips any physics simulation and teleports the joint to the new position."}, + {"getJointInfo", pybullet_getJointInfo, METH_VARARGS, "Get the joint metadata info for a joint on a body. This includes joint index, name, type, q-index and u-index."}, @@ -866,9 +864,6 @@ static PyMethodDef SpamMethods[] = { {"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS, "Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."}, - - {"getNumsetGravity", pybullet_setGravity, METH_VARARGS, - "Set the gravity acceleration (x,y,z)."}, {"getNumJoints", pybullet_getNumJoints, METH_VARARGS, "Get the number of joints for an object."}, From 934725554f61ee567e76207d13c6d8e7e6c9447a Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Tue, 14 Jun 2016 15:21:50 -0700 Subject: [PATCH 4/5] remove initializeJointPosition as setting one joint is not supported --- examples/pybullet/pybullet.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b645045b3..b8bf684c2 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -479,7 +479,7 @@ pybullet_initializeJointPosition(PyObject* self, PyObject* args) { b3SharedMemoryCommandHandle cmd_handle = b3CreatePoseCommandInit(sm, bodyIndex); - printf("initializing joint %d at %f\n", jointIndex, jointPos); + // printf("initializing joint %d at %f\n", jointIndex, jointPos); b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos); b3SharedMemoryStatusHandle status_handle = @@ -850,9 +850,9 @@ static PyMethodDef SpamMethods[] = { {"initializeJointPositions", pybullet_initializeJointPositions, METH_VARARGS, "Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."}, - {"initializeJointPosition", pybullet_initializeJointPosition, METH_VARARGS, - "Initialize the joint position for one joint. This method skips any physics simulation and teleports the joint to the new position."}, - + // {"initializeJointPosition", pybullet_initializeJointPosition, METH_VARARGS, + // "Initialize the joint position for one joint. This method skips any physics simulation and teleports the joint to the new position."}, + // {"getJointInfo", pybullet_getJointInfo, METH_VARARGS, "Get the joint metadata info for a joint on a body. This includes joint index, name, type, q-index and u-index."}, From 03fded3dc7b6d730c7fcaad083115689387f9837 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Wed, 15 Jun 2016 14:21:04 -0700 Subject: [PATCH 5/5] getJointPositions returns a list of all joint positions for a given bodyIndex --- examples/pybullet/pybullet.c | 180 +++++++++++++++++++++++++++-------- 1 file changed, 142 insertions(+), 38 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b8bf684c2..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) @@ -337,6 +337,7 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double baseOrientation[1] = actualStateQ[4]; baseOrientation[2] = actualStateQ[5]; baseOrientation[3] = actualStateQ[6]; + } } } @@ -451,52 +452,146 @@ pybullet_initializeJointPositions(PyObject* self, PyObject* args) return Py_None; } + +// 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): test accuracy of joint position values -static PyObject* -pybullet_initializeJointPosition(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +// 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; - 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); - - } + for (i =0;i