Adds support for loading user data from URDF files.

data/user_data_test_object.urdf

@@ -0,0 +1,100 @@
+<?xml version="1.0" ?>
+<robot name="cube">
+  <bullet>
+    <user-data key="userDataInRobot">
+      Inside the robot tag. Expected identifier: linkIndex: -1, visualShapeIndex: -1
+    </user-data>
+  </bullet>
+  <link name="baseLink">
+    <bullet>
+      <user-data key="userDataInBaseLink">
+        Inside the base link. Expected identifier: linkIndex: -1, visualShapeIndex: -1
+      </user-data>
+    </bullet>
+    <bullet>
+      <user-data key="secondBulletTagInBaseLink1">
+        Second bullet tag inside the base link. Expected identifier: linkIndex: -1, visualShapeIndex: -1
+      </user-data>
+      <user-data key="secondBulletTagInBaseLink2">
+        Second bullet tag inside the base link. Expected identifier: linkIndex: -1, visualShapeIndex: -1
+      </user-data>
+    </bullet>
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <contact_cfm value="0.0"/>
+      <contact_erp value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="1.0"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <bullet>
+        <user-data key="userDataInVisualShape">
+          This is user data inside the visual shape. Expected identifier: linkIndex: -1, visualShapeIndex: 0
+        </user-data>
+      </bullet>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="cube.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<box size="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="childLink">
+    <bullet>
+      <user-data key="userDataInChildLink">
+        Inside the robot tag. Expected identifier: linkIndex: 0, visualShapeIndex: -1
+      </user-data>
+    </bullet>
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <contact_cfm value="0.0"/>
+      <contact_erp value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="1.0"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <bullet>
+        <user-data key="userDataInVisualShape2">
+          This is user data inside the visual shape. Expected identifier: linkIndex: 0, visualShapeIndex: 0
+        </user-data>
+      </bullet>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="cube.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<box size="1 1 1"/>
+      </geometry>
+    </collision>
+    </link>
+      <joint name="joint" type="fixed">
+      <parent link="baseLink"/>
+      <child link="childLink"/>
+      <dynamics damping="1.0" friction="0.0001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+</robot>
+

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -1498,3 +1498,7 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn
 
 	return compoundShape;
 }
+
+const struct UrdfModel* BulletURDFImporter::getUrdfModel() const {
+	return &m_data->m_urdfParser.getModel();
+};

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -74,6 +74,8 @@ public:
 
 	///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
 
+	virtual const struct UrdfModel* getUrdfModel() const;
+
 	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
 
 	virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -85,7 +85,9 @@ public:
 	{
 		return 0;
 	}
-	
+
+	virtual const struct UrdfModel* getUrdfModel() const { return 0; };
+
 	virtual int getNumAllocatedCollisionShapes() const { return 0; }
 	virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/) { return 0; }
 	virtual int getNumModels() const { return 0; }

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -77,6 +77,28 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLo
 	return true;
 }
 
+// Parses user data from an xml element and stores it in a hashmap. User data is
+// expected to reside in a <user-data> tag that is nested inside a <bullet> tag.
+// Example:
+// <bullet>
+//   <user-data key="label">...</user-data>
+// </bullet>
+static void ParseUserData(const XMLElement* element, btHashMap<btHashString,
+	std::string>& user_data, ErrorLogger* logger) {
+	// Parse any custom Bullet-specific info.
+	for (const XMLElement* bullet_xml = element->FirstChildElement("bullet");
+			bullet_xml; bullet_xml = bullet_xml->NextSiblingElement("bullet")) {
+		for (const XMLElement* user_data_xml = bullet_xml->FirstChildElement("user-data");
+				user_data_xml; user_data_xml = user_data_xml->NextSiblingElement("user-data")) {
+			const char* key_attr = user_data_xml->Attribute("key");
+			if (!key_attr) {
+				logger->reportError("User data tag must have a key attribute.");
+			}
+			user_data.insert(key_attr, user_data_xml->GetText());
+		}
+	}
+}
+
 bool UrdfParser::parseMaterial(UrdfMaterial& material, XMLElement* config, ErrorLogger* logger)
 {
 	if (!config->Attribute("name"))
@@ -732,6 +754,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, XMLElement* c
 			}
 		}
 	}
+	ParseUserData(config, visual.m_userData, logger);
 
 	return true;
 }
@@ -1071,6 +1094,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, XMLElement* config,
 			return false;
 		}
 	}
+	ParseUserData(config, link.m_userData, logger);
 	return true;
 }
 
@@ -1781,6 +1805,8 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
 		}
 	}
 
+	ParseUserData(robot_xml, m_urdf2Model.m_userData, logger);
+
 	if (m_urdf2Model.m_links.size() == 0)
 	{
 		logger->reportWarning("No links found in URDF file");

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -127,6 +127,8 @@ struct UrdfShape
 struct UrdfVisual : UrdfShape
 {
 	std::string m_materialName;
+	// Maps user data keys to user data values.
+	btHashMap<btHashString, std::string> m_userData;
 };
 
 struct UrdfCollision : UrdfShape
@@ -160,6 +162,8 @@ struct UrdfLink
 	URDFLinkContactInfo m_contactInfo;
 
 	SDFAudioSource m_audioSource;
+	// Maps user data keys to user data values.
+	btHashMap<btHashString, std::string> m_userData;
 
 	UrdfLink()
 		: m_parentLink(0),
@@ -204,6 +208,8 @@ struct UrdfModel
 	btHashMap<btHashString, UrdfMaterial*> m_materials;
 	btHashMap<btHashString, UrdfLink*> m_links;
 	btHashMap<btHashString, UrdfJoint*> m_joints;
+	// Maps user data keys to user data values.
+	btHashMap<btHashString, std::string> m_userData;
 
 	btArray<UrdfLink*> m_rootLinks;
 	bool m_overrideFixedBase;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -31,6 +31,7 @@
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "Bullet3Common/b3HashMap.h"
 #include "../Utils/ChromeTraceUtil.h"
+#include "SharedMemoryPublic.h"
 #include "stb_image/stb_image.h"
 #include "BulletInverseDynamics/MultiBodyTree.hpp"
 #include "IKTrajectoryHelper.h"
@@ -2363,7 +2364,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 				jointType = URDFFixedJoint;
 				break;
 			}
-			case	eSphericalType:
+			case eSphericalType:
 			{
 				isValid = true;
 				jointType = URDFSphericalJoint;
@@ -3013,6 +3014,46 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
 	}
 }
 
+int PhysicsServerCommandProcessor::addUserData(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, const char* valueBytes, int valueLength, int valueType) {
+	InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+	if (!body)
+	{
+		return -1;
+	}
+
+	SharedMemoryUserDataHashKey userDataIdentifier(key, bodyUniqueId, linkIndex, visualShapeIndex);
+
+	int* userDataHandlePtr = m_data->m_userDataHandleLookup.find(userDataIdentifier);
+	int userDataHandle = userDataHandlePtr ? *userDataHandlePtr : m_data->m_userDataHandles.allocHandle();
+
+	SharedMemoryUserData* userData = m_data->m_userDataHandles.getHandle(userDataHandle);
+	if (!userData)
+	{
+		return -1;
+	}
+
+	if (!userDataHandlePtr)
+	{
+		userData->m_key = key;
+		userData->m_bodyUniqueId = bodyUniqueId;
+		userData->m_linkIndex = linkIndex;
+		userData->m_visualShapeIndex = visualShapeIndex;
+		m_data->m_userDataHandleLookup.insert(userDataIdentifier, userDataHandle);
+		body->m_userDataHandles.push_back(userDataHandle);
+	}
+	userData->replaceValue(valueBytes, valueLength, valueType);
+	return userDataHandle;
+}
+
+void PhysicsServerCommandProcessor::addUserData(const btHashMap<btHashString, std::string>& user_data_entries, int bodyUniqueId, int linkIndex, int visualShapeIndex) {
+	for (int i = 0; i < user_data_entries.size(); ++i) {
+		const std::string key = user_data_entries.getKeyAtIndex(i).m_string1;
+		const std::string* value = user_data_entries.getAtIndex(i);
+		addUserData(bodyUniqueId, linkIndex, visualShapeIndex, key.c_str(), value->c_str(),
+					value->size()+1, USER_DATA_VALUE_TYPE_STRING);
+	}
+}
+
 bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, URDFImporterInterface& u2b)
 {
 	bool loadOk = true;
@@ -3287,6 +3328,18 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
 			}
 		}
 
+	const UrdfModel* urdfModel = u2b.getUrdfModel();
+	if (urdfModel) {
+		addUserData(urdfModel->m_userData, bodyUniqueId);
+		for (int linkIndex = 0; linkIndex < urdfModel->m_links.size(); ++linkIndex) {
+			const UrdfLink* link = *urdfModel->m_links.getAtIndex(linkIndex);
+			addUserData(link->m_userData, bodyUniqueId, linkIndex - 1);
+			for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) {
+				addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, linkIndex - 1, visualShapeIndex);
+			}
+		}
+	}
+
 		b3Notification notification;
 		notification.m_notificationType = BODY_ADDED;
 		notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
@@ -6134,39 +6187,15 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share
 	{
 		return hasStatus;
 	}
-
-	InternalBodyData* body = m_data->m_bodyHandles.getHandle(addUserDataArgs.m_bodyUniqueId);
-	if (!body)
-	{
+	int userDataHandle = addUserData(
+		addUserDataArgs.m_bodyUniqueId, addUserDataArgs.m_linkIndex,
+		addUserDataArgs.m_visualShapeIndex, addUserDataArgs.m_key,
+		bufferServerToClient, addUserDataArgs.m_valueLength,
+		addUserDataArgs.m_valueType);
+	if (userDataHandle < 0) {
 		return hasStatus;
 	}
 
-	SharedMemoryUserDataHashKey userDataIdentifier(
-		addUserDataArgs.m_key,
-		addUserDataArgs.m_bodyUniqueId,
-		addUserDataArgs.m_linkIndex,
-		addUserDataArgs.m_visualShapeIndex);
-
-	int* userDataHandlePtr = m_data->m_userDataHandleLookup.find(userDataIdentifier);
-	int userDataHandle = userDataHandlePtr ? *userDataHandlePtr : m_data->m_userDataHandles.allocHandle();
-
-	SharedMemoryUserData* userData = m_data->m_userDataHandles.getHandle(userDataHandle);
-	if (!userData)
-	{
-		return hasStatus;
-	}
-
-	if (!userDataHandlePtr)
-	{
-		userData->m_key = addUserDataArgs.m_key;
-		userData->m_bodyUniqueId = addUserDataArgs.m_bodyUniqueId;
-		userData->m_linkIndex = addUserDataArgs.m_linkIndex;
-		userData->m_visualShapeIndex = addUserDataArgs.m_visualShapeIndex;
-		m_data->m_userDataHandleLookup.insert(userDataIdentifier, userDataHandle);
-		body->m_userDataHandles.push_back(userDataHandle);
-	}
-	userData->replaceValue(bufferServerToClient, addUserDataArgs.m_valueLength, addUserDataArgs.m_valueType);
-
 	serverStatusOut.m_type = CMD_ADD_USER_DATA_COMPLETED;
 	UserDataResponseArgs& userDataResponseArgs = serverStatusOut.m_userDataResponseArgs;
 	userDataResponseArgs.m_userDataId = userDataHandle;
@@ -9362,6 +9391,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 					gforce->m_gravity = grav;
 				}
 			}
+
 		}
 		
 

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -1,6 +1,7 @@
 #ifndef PHYSICS_SERVER_COMMAND_PROCESSOR_H
 #define PHYSICS_SERVER_COMMAND_PROCESSOR_H
 
+#include "LinearMath/btHashMap.h"
 #include "LinearMath/btVector3.h"
 
 #include "PhysicsCommandProcessorInterface.h"
@@ -180,6 +181,8 @@ public:
 
 private:
 	void addBodyChangedNotifications();
+	int addUserData(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, const char* valueBytes, int valueLength, int valueType);
+	void addUserData(const btHashMap<btHashString, std::string>& user_data_entries, int bodyUniqueId, int linkIndex = -1, int visualShapeIndex = -1);
 };
 
 #endif  //PHYSICS_SERVER_COMMAND_PROCESSOR_H

examples/pybullet/unittests/userDataTest.py

@@ -5,6 +5,7 @@ from pybullet_utils import bullet_client
 
 PLANE_PATH = "plane.urdf"
 ROBOT_PATH = "r2d2.urdf"
+OBJECT_WITH_USER_DATA_PATH = "user_data_test_object.urdf"
 
 
 class TestUserDataMethods(unittest.TestCase):
@@ -25,6 +26,29 @@ class TestUserDataMethods(unittest.TestCase):
     self.client.resetSimulation()
     del self.client
 
+  def testLoadingUserDataFromURDF(self):
+    body_id = self.client.loadURDF(OBJECT_WITH_USER_DATA_PATH)
+    self.client.syncUserData()
+    num_user_data = self.client.getNumUserData(body_id)
+
+    self.assertEqual(num_user_data, 7)
+    expected_user_data_infos = {
+        0 : (b"userDataInRobot", 0, -1, -1),
+        1:( b"userDataInBaseLink", 0, -1, -1),
+        2:( b"secondBulletTagInBaseLink1", 0, -1, -1),
+        3:( b"secondBulletTagInBaseLink2", 0, -1, -1),
+        4:( b"userDataInVisualShape", 0, -1, 0),
+        5:( b"userDataInChildLink", 0, 0, -1),
+        6:( b"userDataInVisualShape2", 0, 0, 0),
+    }
+    for user_data_index in range(num_user_data):
+      info = self.client.getUserDataInfo(body_id, user_data_index)
+      self.assertEqual(info[1:], expected_user_data_infos[info[0]])
+      user_data_val = self.client.getUserData(info[0])
+      self.assertIn(
+          "Expected identifier: linkIndex: %i, visualShapeIndex: %i" %
+          (info[3], info[4]), user_data_val)
+
   def testAddUserData(self):
     plane_id = self.client.loadURDF(PLANE_PATH)
     uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")