1 #ifndef URDF_PARSER_H
2 #define URDF_PARSER_H
3 
4 #include "LinearMath/btTransform.h"
5 #include "LinearMath/btAlignedObjectArray.h"
6 #include "LinearMath/btHashMap.h"
7 #include "URDFJointTypes.h"
8 #include "SDFAudioTypes.h"
9 #include <string>
10 
11 #define btArray btAlignedObjectArray
12 
13 struct ErrorLogger
14 {
~ErrorLoggerErrorLogger15 	virtual ~ErrorLogger() {}
16 	virtual void reportError(const char* error) = 0;
17 	virtual void reportWarning(const char* warning) = 0;
18 	virtual void printMessage(const char* msg) = 0;
19 };
20 
21 struct UrdfMaterial
22 {
23 	std::string m_name;
24 	std::string m_textureFilename;
25 	UrdfMaterialColor m_matColor;
26 
UrdfMaterialUrdfMaterial27 	UrdfMaterial()
28 	{
29 	}
30 };
31 
32 struct UrdfInertia
33 {
34 	btTransform m_linkLocalFrame;
35 	bool m_hasLinkLocalFrame;
36 
37 	double m_mass;
38 	double m_ixx, m_ixy, m_ixz, m_iyy, m_iyz, m_izz;
39 
UrdfInertiaUrdfInertia40 	UrdfInertia()
41 	{
42 		m_hasLinkLocalFrame = false;
43 		m_linkLocalFrame.setIdentity();
44 		m_mass = 0.f;
45 		m_ixx = m_ixy = m_ixz = m_iyy = m_iyz = m_izz = 0.f;
46 	}
47 };
48 
49 enum UrdfGeomTypes
50 {
51 	URDF_GEOM_SPHERE = 2,
52 	URDF_GEOM_BOX,
53 	URDF_GEOM_CYLINDER,
54 	URDF_GEOM_MESH,
55 	URDF_GEOM_PLANE,
56 	URDF_GEOM_CAPSULE,  //non-standard URDF
57 	URDF_GEOM_CDF,      //signed-distance-field, non-standard URDF
58 	URDF_GEOM_HEIGHTFIELD,   //heightfield, non-standard URDF
59 	URDF_GEOM_UNKNOWN,
60 };
61 
62 struct UrdfGeometry
63 {
64 	UrdfGeomTypes m_type;
65 
66 	double m_sphereRadius;
67 
68 	btVector3 m_boxSize;
69 
70 	double m_capsuleRadius;
71 	double m_capsuleHeight;
72 	int m_hasFromTo;
73 	btVector3 m_capsuleFrom;
74 	btVector3 m_capsuleTo;
75 
76 	btVector3 m_planeNormal;
77 
78 	enum
79 	{
80 		FILE_STL = 1,
81 		FILE_COLLADA = 2,
82 		FILE_OBJ = 3,
83 		FILE_CDF = 4,
84 		MEMORY_VERTICES = 5,
85 	        FILE_VTK = 6,
86 
87 	};
88 	int m_meshFileType;
89 	std::string m_meshFileName;
90 	btVector3 m_meshScale;
91 
92 	btArray<btVector3> m_vertices;
93 	btArray<btVector3> m_uvs;
94 	btArray<btVector3> m_normals;
95 	btArray<int> m_indices;
96 
97 
98 	UrdfMaterial m_localMaterial;
99 	bool m_hasLocalMaterial;
100 
UrdfGeometryUrdfGeometry101 	UrdfGeometry()
102 		: m_type(URDF_GEOM_UNKNOWN),
103 		  m_sphereRadius(1),
104 		  m_boxSize(1, 1, 1),
105 		  m_capsuleRadius(1),
106 		  m_capsuleHeight(1),
107 		  m_hasFromTo(0),
108 		  m_capsuleFrom(0, 1, 0),
109 		  m_capsuleTo(1, 0, 0),
110 		  m_planeNormal(0, 0, 1),
111 		  m_meshFileType(0),
112 		  m_meshScale(1, 1, 1),
113 		  m_hasLocalMaterial(false)
114 	{
115 	}
116 };
117 
118 
119 struct UrdfShape
120 {
121 	std::string m_sourceFileLocation;
122 	btTransform m_linkLocalFrame;
123 	UrdfGeometry m_geometry;
124 	std::string m_name;
125 };
126 
127 struct UrdfVisual : UrdfShape
128 {
129 	std::string m_materialName;
130 	// Maps user data keys to user data values.
131 	btHashMap<btHashString, std::string> m_userData;
132 };
133 
134 struct UrdfCollision : UrdfShape
135 {
136 	int m_flags;
137 	int m_collisionGroup;
138 	int m_collisionMask;
UrdfCollisionUrdfCollision139 	UrdfCollision()
140 		: m_flags(0)
141 	{
142 	}
143 };
144 
145 struct UrdfJoint;
146 
147 struct UrdfLink
148 {
149 	std::string m_name;
150 	UrdfInertia m_inertia;
151 	btTransform m_linkTransformInWorld;
152 	btArray<UrdfVisual> m_visualArray;
153 	btArray<UrdfCollision> m_collisionArray;
154 	UrdfLink* m_parentLink;
155 	UrdfJoint* m_parentJoint;
156 
157 	btArray<UrdfJoint*> m_childJoints;
158 	btArray<UrdfLink*> m_childLinks;
159 
160 	int m_linkIndex;
161 
162 	URDFLinkContactInfo m_contactInfo;
163 
164 	SDFAudioSource m_audioSource;
165 	// Maps user data keys to user data values.
166 	btHashMap<btHashString, std::string> m_userData;
167 
UrdfLinkUrdfLink168 	UrdfLink()
169 		: m_parentLink(0),
170 		  m_parentJoint(0),
171 		  m_linkIndex(-2)
172 	{
173 	}
174 };
175 struct UrdfJoint
176 {
177 	std::string m_name;
178 	UrdfJointTypes m_type;
179 	btTransform m_parentLinkToJointTransform;
180 	std::string m_parentLinkName;
181 	std::string m_childLinkName;
182 	btVector3 m_localJointAxis;
183 
184 	double m_lowerLimit;
185 	double m_upperLimit;
186 
187 	double m_effortLimit;
188 	double m_velocityLimit;
189 
190 	double m_jointDamping;
191 	double m_jointFriction;
UrdfJointUrdfJoint192 	UrdfJoint()
193 		: m_lowerLimit(0),
194 		  m_upperLimit(-1),
195 		  m_effortLimit(0),
196 		  m_velocityLimit(0),
197 		  m_jointDamping(0),
198 		  m_jointFriction(0)
199 	{
200 	}
201 };
202 
203 struct SpringCoeffcients
204 {
205 	double elastic_stiffness;
206 	double damping_stiffness;
207 	double bending_stiffness;
208 	int damp_all_directions;
209 	int bending_stride;
SpringCoeffcientsSpringCoeffcients210 	SpringCoeffcients() : elastic_stiffness(0.),
211 						  damping_stiffness(0.),
212 						  bending_stiffness(0.),
213 						  damp_all_directions(0),
214 						  bending_stride(2) {}
215 };
216 
217 struct LameCoefficients
218 {
219 	double mu;
220 	double lambda;
221 	double damping;
LameCoefficientsLameCoefficients222 	LameCoefficients() : mu(0.), lambda(0.), damping(0.) {}
223 };
224 
225 struct UrdfDeformable
226 {
227 	std::string m_name;
228 	double m_mass;
229 	double m_collisionMargin;
230 	double m_friction;
231 	double m_repulsionStiffness;
232 	double m_gravFactor;
233 	bool m_cache_barycenter;
234 
235 	SpringCoeffcients m_springCoefficients;
236 	LameCoefficients m_corotatedCoefficients;
237 	LameCoefficients m_neohookeanCoefficients;
238 
239 	std::string m_visualFileName;
240 	std::string m_simFileName;
241 	btHashMap<btHashString, std::string> m_userData;
242 
UrdfDeformableUrdfDeformable243 	UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_gravFactor(1.), m_cache_barycenter(false), m_visualFileName(""), m_simFileName("")
244 	{
245 	}
246 };
247 
248 struct UrdfModel
249 {
250 	std::string m_name;
251 	std::string m_sourceFile;
252 	btTransform m_rootTransformInWorld;
253 	btHashMap<btHashString, UrdfMaterial*> m_materials;
254 	btHashMap<btHashString, UrdfLink*> m_links;
255 	btHashMap<btHashString, UrdfJoint*> m_joints;
256 	UrdfDeformable m_deformable;
257 	// Maps user data keys to user data values.
258 	btHashMap<btHashString, std::string> m_userData;
259 
260 	btArray<UrdfLink*> m_rootLinks;
261 	bool m_overrideFixedBase;
262 
UrdfModelUrdfModel263 	UrdfModel()
264 		: m_overrideFixedBase(false)
265 	{
266 		m_rootTransformInWorld.setIdentity();
267 	}
268 
~UrdfModelUrdfModel269 	~UrdfModel()
270 	{
271 		for (int i = 0; i < m_materials.size(); i++)
272 		{
273 			UrdfMaterial** ptr = m_materials.getAtIndex(i);
274 			if (ptr)
275 			{
276 				UrdfMaterial* t = *ptr;
277 				delete t;
278 			}
279 		}
280 		for (int i = 0; i < m_links.size(); i++)
281 		{
282 			UrdfLink** ptr = m_links.getAtIndex(i);
283 			if (ptr)
284 			{
285 				UrdfLink* t = *ptr;
286 				delete t;
287 			}
288 		}
289 		for (int i = 0; i < m_joints.size(); i++)
290 		{
291 			UrdfJoint** ptr = m_joints.getAtIndex(i);
292 			if (ptr)
293 			{
294 				UrdfJoint* t = *ptr;
295 				delete t;
296 			}
297 		}
298 	}
299 };
300 
301 namespace tinyxml2
302 {
303 class XMLElement;
304 };
305 
306 class UrdfParser
307 {
308 protected:
309 	UrdfModel m_urdf2Model;
310 	btAlignedObjectArray<UrdfModel*> m_sdfModels;
311 	btAlignedObjectArray<UrdfModel*> m_tmpModels;
312 
313 	bool m_parseSDF;
314 	int m_activeSdfModel;
315 
316 	btScalar m_urdfScaling;
317 
318 	struct CommonFileIOInterface* m_fileIO;
319 
320 	bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
321 	bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
322 	bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, ErrorLogger* logger);
323 	bool parseVisual(UrdfModel& model, UrdfVisual& visual, tinyxml2::XMLElement* config, ErrorLogger* logger);
324 	bool parseCollision(UrdfCollision& collision, tinyxml2::XMLElement* config, ErrorLogger* logger);
325 	bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
326 	bool parseMaterial(UrdfMaterial& material, tinyxml2::XMLElement* config, ErrorLogger* logger);
327 	bool parseJointLimits(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
328 	bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
329 	bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
330 	bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger);
331 	bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
332   bool parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger);
333 	bool parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
334 
335 
336 public:
337 	UrdfParser(struct CommonFileIOInterface* fileIO);
338 	virtual ~UrdfParser();
339 
setParseSDF(bool useSDF)340 	void setParseSDF(bool useSDF)
341 	{
342 		m_parseSDF = useSDF;
343 	}
getParseSDF()344 	bool getParseSDF() const
345 	{
346 		return m_parseSDF;
347 	}
setGlobalScaling(btScalar scaling)348 	void setGlobalScaling(btScalar scaling)
349 	{
350 		m_urdfScaling = scaling;
351 	}
352 
353 	bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase, bool parseSensors);
354 
loadUrdf(const char * urdfText,ErrorLogger * logger,bool forceFixedBase)355 	bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase)
356 	{
357 		return loadUrdf(urdfText, logger, forceFixedBase, false);
358 	}
359 
360 	bool loadSDF(const char* sdfText, ErrorLogger* logger);
361 
getNumModels()362 	int getNumModels() const
363 	{
364 		//user should have loaded an SDF when calling this method
365 		if (m_parseSDF)
366 		{
367 			return m_sdfModels.size();
368 		}
369 		return 1;
370 	}
371 
372 	void activateModel(int modelIndex);
373 
getModelByIndex(int index)374 	UrdfModel& getModelByIndex(int index)
375 	{
376 		//user should have loaded an SDF when calling this method
377 		btAssert(m_parseSDF);
378 
379 		return *m_sdfModels[index];
380 	}
381 
getModelByIndex(int index)382 	const UrdfModel& getModelByIndex(int index) const
383 	{
384 		//user should have loaded an SDF when calling this method
385 		btAssert(m_parseSDF);
386 
387 		return *m_sdfModels[index];
388 	}
389 
getModel()390 	const UrdfModel& getModel() const
391 	{
392 		if (m_parseSDF)
393 		{
394 			return *m_sdfModels[m_activeSdfModel];
395 		}
396 
397 		return m_urdf2Model;
398 	}
399 
getModel()400 	UrdfModel& getModel()
401 	{
402 		if (m_parseSDF)
403 		{
404 			return *m_sdfModels[m_activeSdfModel];
405 		}
406 		return m_urdf2Model;
407 	}
408 
getDeformable()409 	const UrdfDeformable& getDeformable() const
410 	{
411 		return m_urdf2Model.m_deformable;
412 	}
413 
414 	bool mergeFixedLinks(UrdfModel& model, UrdfLink* link, ErrorLogger* logger, bool forceFixedBase, int level);
415 	bool printTree(UrdfLink* link, ErrorLogger* logger, int level);
416 	bool recreateModel(UrdfModel& model, UrdfLink* link, ErrorLogger* logger);
417 
418 	std::string sourceFileLocation(tinyxml2::XMLElement* e);
419 
setSourceFile(const std::string & sourceFile)420 	void setSourceFile(const std::string& sourceFile)
421 	{
422 		m_urdf2Model.m_sourceFile = sourceFile;
423 	}
424 };
425 
426 #endif
427