diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 829eeca56..466eeb293 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2052,7 +2052,8 @@ B3_SHARED_API int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandl const double linkInertialFrameOrientation[4], int linkParentIndex, int linkJointType, - const double linkJointAxis[3]) + const double linkJointAxis[3], + const char* const linkName) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command); @@ -2097,6 +2098,7 @@ B3_SHARED_API int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandl command->m_createMultiBodyArgs.m_linkJointAxis[3 * linkIndex + 2] = linkJointAxis[2]; command->m_createMultiBodyArgs.m_linkMasses[linkIndex] = linkMass; + command->m_createMultiBodyArgs.m_linkNames[linkIndex] = linkName; command->m_createMultiBodyArgs.m_numLinks++; return numLinks; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 318263b2d..7c7ac1442 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -552,7 +552,8 @@ extern "C" const double linkInertialFrameOrientation[/*4*/], int linkParentIndex, int linkJointType, - const double linkJointAxis[/*3*/]); + const double linkJointAxis[/*3*/], + const char* const linkName); //batch creation is an performance feature to create a large number of multi bodies in one command B3_SHARED_API int b3CreateMultiBodySetBatchPositions(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, double* batchPositions, int numBatchObjects); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 295e202c5..f358b5d23 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2232,6 +2232,11 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range) virtual std::string getLinkName(int linkIndex) const { + // Allow user overrides on default-created link names. + if(m_createBodyArgs.m_linkNames[linkIndex] != nullptr + && strlen(m_createBodyArgs.m_linkNames[linkIndex]) > 0){ + return std::string(m_createBodyArgs.m_linkNames[linkIndex]); + } std::string linkName = "link"; char numstr[21]; // enough to hold all numbers up to 64-bits sprintf(numstr, "%d", linkIndex); @@ -2309,6 +2314,11 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface virtual std::string getJointName(int linkIndex) const { + // Allow user overrides on default-created joint names. + if(m_createBodyArgs.m_linkNames[linkIndex] != nullptr + && strlen(m_createBodyArgs.m_linkNames[linkIndex]) > 0){ + return std::string(m_createBodyArgs.m_linkNames[linkIndex]); + } std::string jointName = "joint"; char numstr[21]; // enough to hold all numbers up to 64-bits sprintf(numstr, "%d", linkIndex); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index a019a58ca..311379d79 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -1057,6 +1057,7 @@ struct b3CreateMultiBodyArgs int m_flags; int m_numBatchObjects; + const char* m_linkNames[MAX_CREATE_MULTI_BODY_LINKS]; }; struct b3CreateMultiBodyResultArgs diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index f2777e129..4eb95c4b3 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -2431,7 +2431,8 @@ int b3RobotSimulatorClientAPI_NoDirect::createMultiBody(struct b3RobotSimulatorC doubleLinkInertialFrameOrientation, linkParentIndex, linkJointType, - doubleLinkJointAxis); + doubleLinkJointAxis, + 0); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 56b5f7511..b6387f3c6 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -128,6 +128,24 @@ static int pybullet_internalGetIntFromSequence(PyObject* seq, int index) return v; } +static const char* pybullet_internalGetCStringFromSequence(PyObject* seq, int index) +{ + const char* v = 0; + PyObject* item; + + if (PyList_Check(seq)) + { + item = PyList_GET_ITEM(seq, index); + v = PyUnicode_AsUTF8(item); + } + else + { + item = PyTuple_GET_ITEM(seq, index); + v = PyUnicode_AsUTF8(item); + } + return v; +} + // internal function to set a float matrix[16] // used to initialize camera position with // a view and projection matrix in renderImage() @@ -9350,18 +9368,19 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje PyObject* linkInertialFramePositionObj = 0; PyObject* linkInertialFrameOrientationObj = 0; PyObject* objBatchPositions = 0; + PyObject* linkNamesObj = 0; static char* kwlist[] = { "baseMass", "baseCollisionShapeIndex", "baseVisualShapeIndex", "basePosition", "baseOrientation", "baseInertialFramePosition", "baseInertialFrameOrientation", "linkMasses", "linkCollisionShapeIndices", "linkVisualShapeIndices", "linkPositions", "linkOrientations", "linkInertialFramePositions", "linkInertialFrameOrientations", "linkParentIndices", - "linkJointTypes", "linkJointAxis", "useMaximalCoordinates", "flags", "batchPositions", "physicsClientId", NULL}; + "linkJointTypes", "linkJointAxis", "useMaximalCoordinates", "flags", "batchPositions", "linkNames", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOOOiiOi", kwlist, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOOOiiOOi", kwlist, &baseMass, &baseCollisionShapeIndex, &baseVisualShapeIndex, &basePosObj, &baseOrnObj, &baseInertialFramePositionObj, &baseInertialFrameOrientationObj, &linkMassesObj, &linkCollisionShapeIndicesObj, &linkVisualShapeIndicesObj, &linkPositionsObj, &linkOrientationsObj, &linkInertialFramePositionObj, &linkInertialFrameOrientationObj, &linkParentIndicesObj, - &linkJointTypesObj, &linkJointAxisObj, &useMaximalCoordinates, &flags, &objBatchPositions, &physicsClientId)) + &linkJointTypesObj, &linkJointAxisObj, &useMaximalCoordinates, &flags, &objBatchPositions, &linkNamesObj, &physicsClientId)) { return NULL; } @@ -9398,6 +9417,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje PyObject* seqLinkInertialFrameOrientations = linkInertialFrameOrientationObj ? PySequence_Fast(linkInertialFrameOrientationObj, "expected a sequence") : 0; PyObject* seqBatchPositions = objBatchPositions ? PySequence_Fast(objBatchPositions, "expected a sequence") : 0; + PyObject* seqLinkNames = linkNamesObj ? PySequence_Fast(linkNamesObj, "expected a sequence") : 0; if ((numLinkMasses == numLinkCollisionShapes) && (numLinkMasses == numLinkVisualShapes) && @@ -9448,6 +9468,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje double linkInertialFrameOrientation[4]; int linkParentIndex; int linkJointType; + const char* linkName; pybullet_internalGetVector3FromSequence(seqLinkInertialFramePositions, i, linkInertialFramePosition); pybullet_internalGetVector4FromSequence(seqLinkInertialFrameOrientations, i, linkInertialFrameOrientation); @@ -9456,6 +9477,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje pybullet_internalGetVector3FromSequence(seqLinkJoinAxis, i, linkJointAxis); linkParentIndex = pybullet_internalGetIntFromSequence(seqLinkParentIndices, i); linkJointType = pybullet_internalGetIntFromSequence(seqLinkJointTypes, i); + linkName = seqLinkNames? pybullet_internalGetCStringFromSequence(seqLinkNames, i) : 0; b3CreateMultiBodyLink(commandHandle, linkMass, @@ -9467,7 +9489,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje linkInertialFrameOrientation, linkParentIndex, linkJointType, - linkJointAxis); + linkJointAxis, + linkName); } if (seqLinkMasses)