diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index b76e7e18c..3fc0fa16a 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -24,9 +24,15 @@ public: virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0; - virtual int getNumJoints(int bodyIndex) const = 0; + virtual int getNumBodies() const = 0; - virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0; + virtual int getBodyUniqueId(int serialIndex) const = 0; + + virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const = 0; + + virtual int getNumJoints(int bodyUniqueId) const = 0; + + virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0; virtual void setSharedMemoryKey(int key) = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 61a192fa8..a9f7cc56f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -739,6 +739,29 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan } +///return the total number of bodies in the simulation +int b3GetNumBodies(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + return cl->getNumBodies(); +} + +/// return the body unique id, given the index in range [0 , b3GetNumBodies() ) +int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + return cl->getBodyUniqueId(serialIndex); +} + +///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h +int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + return cl->getBodyInfo(bodyUniqueId,*info); +} + + + int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index a57851fd8..d35f472fb 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -53,6 +53,15 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, const double* actualStateQdot[], const double* jointReactionForces[]); +///return the total number of bodies in the simulation +int b3GetNumBodies(b3PhysicsClientHandle physClient); + +/// return the body unique id, given the index in range [0 , b3GetNumBodies() ) +int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex); + +///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h +int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info); + ///give a unique body index (after loading the body) return the number of joints. int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 07ba317a4..8ba6cfca9 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -624,7 +624,7 @@ void PhysicsClientExample::initPhysics() { MyCallback(CMD_LOAD_URDF, true, this); MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); - MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); + MyCallback(CMD_RESET_SIMULATION,true,this); } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index e622cf716..1ed323289 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -18,6 +18,7 @@ struct BodyJointInfoCache { + std::string m_baseName; btAlignedObjectArray m_jointInfo; }; @@ -74,10 +75,37 @@ struct PhysicsClientSharedMemoryInternalData { - -int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const +int PhysicsClientSharedMemory::getNumBodies() const { - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + return m_data->m_bodyJointMap.size(); +} + +int PhysicsClientSharedMemory::getBodyUniqueId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumBodies())) + { + return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const +{ + BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache* bodyJoints = *bodyJointsPtr; + info.m_baseName = bodyJoints->m_baseName.c_str(); + return true; + } + return false; +} + + + +int PhysicsClientSharedMemory::getNumJoints(int bodyUniqueId) const +{ + BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; @@ -88,9 +116,9 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const } -bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const +bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const { - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; @@ -196,12 +224,13 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + bodyJoints->m_baseName = mb->m_baseName; addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } else { Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; - + bodyJoints->m_baseName = mb->m_baseName; addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } } @@ -272,10 +301,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { serverCmd.m_dataStreamArguments.m_streamChunkLength); bf.setFileDNAisMemoryDNA(); bf.parse(false); - int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId; BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; - m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints); + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); for (int i = 0; i < bf.m_multiBodies.size(); i++) { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 69e5ed64d..1d0047bff 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -34,9 +34,15 @@ public: virtual bool submitClientCommand(const struct SharedMemoryCommand& command); - virtual int getNumJoints(int bodyIndex) const; + virtual int getNumBodies() const; - virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual int getBodyUniqueId(int serialIndex) const; + + virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const; + + virtual int getNumJoints(int bodyUniqueId) const; + + virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const; virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 6ac5d6212..3367b8323 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -9,10 +9,11 @@ #include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" #include "BodyJointInfoUtility.h" - +#include struct BodyJointInfoCache2 { + std::string m_baseName; btAlignedObjectArray m_jointInfo; }; @@ -329,8 +330,7 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta serverCmd.m_dataStreamArguments.m_streamChunkLength); bf.setFileDNAisMemoryDNA(); bf.parse(false); - - + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); @@ -341,13 +341,15 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta { Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; - + bodyJoints->m_baseName = mb->m_baseName; + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } else { Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; - + + bodyJoints->m_baseName = mb->m_baseName; addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } } @@ -458,6 +460,34 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman return hasStatus; } +int PhysicsDirect::getNumBodies() const +{ + return m_data->m_bodyJointMap.size(); +} + + +int PhysicsDirect::getBodyUniqueId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumBodies())) + { + return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + info.m_baseName = bodyJoints->m_baseName.c_str(); + return true; + } + + return false; +} + int PhysicsDirect::getNumJoints(int bodyIndex) const { BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index eae128998..e312823e2 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -49,6 +49,12 @@ public: virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + virtual int getNumBodies() const; + + virtual int getBodyUniqueId(int serialIndex) const; + + virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const; + virtual int getNumJoints(int bodyIndex) const; virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 91ac0d2cc..eeb641023 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -74,6 +74,21 @@ bool PhysicsLoopBack::submitClientCommand(const struct SharedMemoryCommand& comm return m_data->m_physicsClient->submitClientCommand(command); } +int PhysicsLoopBack::getNumBodies() const +{ + return m_data->m_physicsClient->getNumBodies(); +} + +int PhysicsLoopBack::getBodyUniqueId(int serialIndex) const +{ + return m_data->m_physicsClient->getBodyUniqueId(serialIndex); +} + +bool PhysicsLoopBack::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const +{ + return m_data->m_physicsClient->getBodyInfo(bodyUniqueId, info); +} + int PhysicsLoopBack::getNumJoints(int bodyIndex) const { return m_data->m_physicsClient->getNumJoints(bodyIndex); diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 326e21128..baa7939fc 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -38,6 +38,12 @@ public: virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + virtual int getNumBodies() const; + + virtual int getBodyUniqueId(int serialIndex) const; + + virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const; + virtual int getNumJoints(int bodyIndex) const; virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9015bdb81..deaa5bda8 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1064,6 +1064,10 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0); + if (mb->getBaseName()) + { + util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName()); + } bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); for (int i=0;igetNumLinks();i++) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index dbb4dcd19..9b1068470 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -117,6 +117,13 @@ struct b3JointInfo double m_jointAxis[3]; // joint axis in parent local frame }; +struct b3BodyInfo +{ + const char* m_baseName; +}; + + + struct b3JointSensorState { double m_jointPosition; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index cfa9ba684..6e010141d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -567,7 +567,7 @@ static int pybullet_internalGetBasePositionAndOrientation( const int status_type = b3GetStatusType(status_handle); const double* actualStateQ; // const double* jointReactionForces[]; - int i; + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); @@ -661,10 +661,88 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, } } +static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args) +{ + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int numBodies = b3GetNumBodies(sm); + +#if PY_MAJOR_VERSION >= 3 + return PyLong_FromLong(numBodies); +#else + return PyInt_FromLong(numBodies); +#endif + } +} + +static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args) +{ + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int serialIndex = -1; + int bodyUniqueId = -1; + if (!PyArg_ParseTuple(args, "i", &serialIndex)) { + PyErr_SetString(SpamError, "Expected a serialIndex in range [0..number of bodies)."); + return NULL; + } + bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex); + +#if PY_MAJOR_VERSION >= 3 + return PyLong_FromLong(bodyUniqueId); +#else + return PyInt_FromLong(bodyUniqueId); +#endif + } +} + +static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args) +{ + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int bodyUniqueId= -1; + int numJoints = 0; + if (!PyArg_ParseTuple(args, "i", &bodyUniqueId)) + { + PyErr_SetString(SpamError, "Expected a body unique id (integer)."); + return NULL; + } + { + struct b3BodyInfo info; + if (b3GetBodyInfo(sm,bodyUniqueId,&info)) + { + PyObject* pyListJointInfo = PyTuple_New(1); + PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName)); + return pyListJointInfo; + } else + { + PyErr_SetString(SpamError, "Couldn't get body info"); + return NULL; + } + } + } + + PyErr_SetString(SpamError, "error in getBodyInfo."); + return NULL; +} + + // 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) { +static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args) +{ if (0 == sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -888,16 +966,15 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) { static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) { PyObject* pyListJointForceTorque; PyObject* pyListJointState; - PyObject* item; - struct b3JointInfo info; + struct b3JointSensorState sensorState; int bodyIndex = -1; int jointIndex = -1; int sensorStateSize = 4; // size of struct b3JointSensorState int forceTorqueSize = 6; // size of force torque list from b3JointSensorState - int i, j; + int j; int size = PySequence_Size(args); @@ -2005,6 +2082,15 @@ static PyMethodDef SpamMethods[] = { {"loadSDF", pybullet_loadSDF, METH_VARARGS, "Load multibodies from an SDF file."}, + {"getNumBodies", pybullet_getNumBodies, METH_VARARGS, + "Get the number of bodies in the simulation."}, + + {"getBodyUniqueId", pybullet_getBodyUniqueId, METH_VARARGS, + "Get the unique id of the body, given a integer serial index in range [0.. number of bodies)."}, + + {"getBodyInfo", pybullet_getBodyInfo, METH_VARARGS, + "Get the body info, given a body unique id."}, + {"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS, "Get the world position and orientation of the base of the object. "