bullet3/examples/Importers/ImportURDFDemo/UrdfParser.h

361 lines
7.1 KiB
C++

#ifndef URDF_PARSER_H
#define URDF_PARSER_H
#include "LinearMath/btTransform.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btHashMap.h"
#include "URDFJointTypes.h"
#include "SDFAudioTypes.h"
#include <string>
#define btArray btAlignedObjectArray
struct ErrorLogger
{
virtual ~ErrorLogger() {}
virtual void reportError(const char* error) = 0;
virtual void reportWarning(const char* warning) = 0;
virtual void printMessage(const char* msg) = 0;
};
struct UrdfMaterial
{
std::string m_name;
std::string m_textureFilename;
UrdfMaterialColor m_matColor;
UrdfMaterial()
{
}
};
struct UrdfInertia
{
btTransform m_linkLocalFrame;
bool m_hasLinkLocalFrame;
double m_mass;
double m_ixx, m_ixy, m_ixz, m_iyy, m_iyz, m_izz;
UrdfInertia()
{
m_hasLinkLocalFrame = false;
m_linkLocalFrame.setIdentity();
m_mass = 0.f;
m_ixx = m_ixy = m_ixz = m_iyy = m_iyz = m_izz = 0.f;
}
};
enum UrdfGeomTypes
{
URDF_GEOM_SPHERE = 2,
URDF_GEOM_BOX,
URDF_GEOM_CYLINDER,
URDF_GEOM_MESH,
URDF_GEOM_PLANE,
URDF_GEOM_CAPSULE, //non-standard URDF
URDF_GEOM_CDF, //signed-distance-field, non-standard URDF
URDF_GEOM_UNKNOWN,
};
struct UrdfGeometry
{
UrdfGeomTypes m_type;
double m_sphereRadius;
btVector3 m_boxSize;
double m_capsuleRadius;
double m_capsuleHeight;
int m_hasFromTo;
btVector3 m_capsuleFrom;
btVector3 m_capsuleTo;
btVector3 m_planeNormal;
enum
{
FILE_STL = 1,
FILE_COLLADA = 2,
FILE_OBJ = 3,
FILE_CDF = 4,
MEMORY_VERTICES = 5,
};
int m_meshFileType;
std::string m_meshFileName;
btVector3 m_meshScale;
btArray<btVector3> m_vertices;
btArray<btVector3> m_uvs;
btArray<btVector3> m_normals;
btArray<int> m_indices;
UrdfMaterial m_localMaterial;
bool m_hasLocalMaterial;
UrdfGeometry()
: m_type(URDF_GEOM_UNKNOWN),
m_sphereRadius(1),
m_boxSize(1, 1, 1),
m_capsuleRadius(1),
m_capsuleHeight(1),
m_hasFromTo(0),
m_capsuleFrom(0, 1, 0),
m_capsuleTo(1, 0, 0),
m_planeNormal(0, 0, 1),
m_meshFileType(0),
m_meshScale(1, 1, 1),
m_hasLocalMaterial(false)
{
}
};
struct UrdfShape
{
std::string m_sourceFileLocation;
btTransform m_linkLocalFrame;
UrdfGeometry m_geometry;
std::string m_name;
};
struct UrdfVisual : UrdfShape
{
std::string m_materialName;
};
struct UrdfCollision : UrdfShape
{
int m_flags;
int m_collisionGroup;
int m_collisionMask;
UrdfCollision()
: m_flags(0)
{
}
};
struct UrdfJoint;
struct UrdfLink
{
std::string m_name;
UrdfInertia m_inertia;
btTransform m_linkTransformInWorld;
btArray<UrdfVisual> m_visualArray;
btArray<UrdfCollision> m_collisionArray;
UrdfLink* m_parentLink;
UrdfJoint* m_parentJoint;
btArray<UrdfJoint*> m_childJoints;
btArray<UrdfLink*> m_childLinks;
int m_linkIndex;
URDFLinkContactInfo m_contactInfo;
SDFAudioSource m_audioSource;
UrdfLink()
: m_parentLink(0),
m_parentJoint(0),
m_linkIndex(-2)
{
}
};
struct UrdfJoint
{
std::string m_name;
UrdfJointTypes m_type;
btTransform m_parentLinkToJointTransform;
std::string m_parentLinkName;
std::string m_childLinkName;
btVector3 m_localJointAxis;
double m_lowerLimit;
double m_upperLimit;
double m_effortLimit;
double m_velocityLimit;
double m_jointDamping;
double m_jointFriction;
UrdfJoint()
: m_lowerLimit(0),
m_upperLimit(-1),
m_effortLimit(0),
m_velocityLimit(0),
m_jointDamping(0),
m_jointFriction(0)
{
}
};
struct UrdfModel
{
std::string m_name;
std::string m_sourceFile;
btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> m_rootLinks;
bool m_overrideFixedBase;
UrdfModel()
: m_overrideFixedBase(false)
{
m_rootTransformInWorld.setIdentity();
}
~UrdfModel()
{
for (int i = 0; i < m_materials.size(); i++)
{
UrdfMaterial** ptr = m_materials.getAtIndex(i);
if (ptr)
{
UrdfMaterial* t = *ptr;
delete t;
}
}
for (int i = 0; i < m_links.size(); i++)
{
UrdfLink** ptr = m_links.getAtIndex(i);
if (ptr)
{
UrdfLink* t = *ptr;
delete t;
}
}
for (int i = 0; i < m_joints.size(); i++)
{
UrdfJoint** ptr = m_joints.getAtIndex(i);
if (ptr)
{
UrdfJoint* t = *ptr;
delete t;
}
}
}
};
namespace tinyxml2
{
class XMLElement;
};
class UrdfParser
{
protected:
UrdfModel m_urdf2Model;
btAlignedObjectArray<UrdfModel*> m_sdfModels;
btAlignedObjectArray<UrdfModel*> m_tmpModels;
bool m_parseSDF;
int m_activeSdfModel;
btScalar m_urdfScaling;
struct CommonFileIOInterface* m_fileIO;
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, ErrorLogger* logger);
bool parseVisual(UrdfModel& model, UrdfVisual& visual, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseCollision(UrdfCollision& collision, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
public:
UrdfParser(struct CommonFileIOInterface* fileIO);
virtual ~UrdfParser();
void setParseSDF(bool useSDF)
{
m_parseSDF = useSDF;
}
bool getParseSDF() const
{
return m_parseSDF;
}
void setGlobalScaling(btScalar scaling)
{
m_urdfScaling = scaling;
}
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase, bool parseSensors);
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase)
{
return loadUrdf(urdfText, logger, forceFixedBase, false);
}
bool loadSDF(const char* sdfText, ErrorLogger* logger);
int getNumModels() const
{
//user should have loaded an SDF when calling this method
if (m_parseSDF)
{
return m_sdfModels.size();
}
return 1;
}
void activateModel(int modelIndex);
UrdfModel& getModelByIndex(int index)
{
//user should have loaded an SDF when calling this method
btAssert(m_parseSDF);
return *m_sdfModels[index];
}
const UrdfModel& getModelByIndex(int index) const
{
//user should have loaded an SDF when calling this method
btAssert(m_parseSDF);
return *m_sdfModels[index];
}
const UrdfModel& getModel() const
{
if (m_parseSDF)
{
return *m_sdfModels[m_activeSdfModel];
}
return m_urdf2Model;
}
UrdfModel& getModel()
{
if (m_parseSDF)
{
return *m_sdfModels[m_activeSdfModel];
}
return m_urdf2Model;
}
std::string sourceFileLocation(tinyxml2::XMLElement* e);
void setSourceFile(const std::string& sourceFile)
{
m_urdf2Model.m_sourceFile = sourceFile;
}
};
#endif