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