Merge pull request #3238 from yycho0108/add-link-names-to-createmultibody

Add linkNames argument to CreateMultibody

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;
 		}

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);

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] != 0
+				&& 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] != 0
+				&& 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);

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

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);