From 4f4eb3a9c6cfe5328a5410b0cce52036f5b11385 Mon Sep 17 00:00:00 2001 From: Giorgos Tzampanakis Date: Tue, 14 Jun 2016 13:40:38 +0000 Subject: [PATCH 1/8] Fixed: Error because of unparameterized btVectorX --- src/BulletInverseDynamics/details/IDLinearMathInterface.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index c77ba3cba..d81647b68 100644 --- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -48,9 +48,9 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } class vecx : public btVectorX { public: - vecx(int size) : btVectorX(size) {} + vecx(int size) : btVectorX(size) {} const vecx& operator=(const btVectorX& rhs) { - *static_cast(this) = rhs; + *static_cast*>(this) = rhs; return *this; } From b06e1cb873546d940003834121e2f928190adddb Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 14 Jun 2016 07:53:15 -0700 Subject: [PATCH 2/8] Update CollisionShape2TriangleMesh.cpp fix memory leak, thanks Ilya Kostrikov for the report. --- examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index 1105955dc..2bc8a0ea4 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -167,6 +167,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans } } } + delete hull; } } else { From eeaff0747b47c46b1eb86e239475475e684762d8 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Tue, 14 Jun 2016 14:08:24 -0700 Subject: [PATCH 3/8] 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 4/8] 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 5/8] 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 6/8] 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 fd88d73e3c0d064ee0815640deb2297954bcf4f4 Mon Sep 17 00:00:00 2001 From: David Date: Wed, 15 Jun 2016 01:36:10 +0100 Subject: [PATCH 7/8] Typos in readme --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 711ebcf98..942500cb9 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ The entire collision detection and rigid body dynamics is executed on the GPU. A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better. We succesfully tested the software under Windows, Linux and Mac OSX. The software currently doesn't work on OpenCL CPU devices. It might run -on a laptop GPU but performance is likely not very good. Note that +on a laptop GPU but performance will not likely be very good. Note that often an OpenCL drivers fails to compile a kernel. Some unit tests exist to track down the issue, but more work is required to cover all OpenCL kernels. @@ -88,7 +88,7 @@ You can just run it though a terminal/command prompt, or by clicking it. [--fixed_timestep= 0.0] Use either a real-time delta time (0.0) or a fixed step size (0.016666) ``` -You can use mouse picking to grab objects. When holding the ALT of CONTROL key, you have Maya style camera mouse controls. -Press F1 to create series of screenshot. Hit ESCAPE to exit the demo app. +You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls. +Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app. See docs folder for further information. From 03fded3dc7b6d730c7fcaad083115689387f9837 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Wed, 15 Jun 2016 14:21:04 -0700 Subject: [PATCH 8/8] 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