Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2016-05-10 10:18:27 -07:00
parent cbeddfc897 119d47166d
commit 1bebcc6d9a
101 changed files with 5737 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -21,7 +21,7 @@ subject to the following restrictions:
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "Bullet3Common/b3FileUtils.h"
#include <string>
#include "../../Utils/b3ResourcePath.h"
@@ -133,11 +133,69 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
}
BulletErrorLogger loggie;
m_data->m_urdfParser.setParseSDF(false);
bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase);
return result;
}
int BulletURDFImporter::getNumModels() const
{
return m_data->m_urdfParser.getNumModels();
}
void BulletURDFImporter::activateModel(int modelIndex)
{
m_data->m_urdfParser.activateModel(modelIndex);
}
bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
{
m_data->m_linkColors.clear();
//int argc=0;
char relativeFileName[1024];
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = b3ResourcePath::findResourcePath(fileName,relativeFileName,1024);
std::string xml_string;
m_data->m_pathPrefix[0] = 0;
if (!fileFound){
std::cerr << "URDF file not found" << std::endl;
return false;
} else
{
int maxPathLen = 1024;
fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
std::fstream xml_file(relativeFileName, std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
}
BulletErrorLogger loggie;
//todo: quick test to see if we can re-use the URDF parser for SDF or not
m_data->m_urdfParser.setParseSDF(true);
bool result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie);
return result;
}
const char* BulletURDFImporter::getPathPrefix()
{
return m_data->m_pathPrefix;

6
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -4,6 +4,7 @@
#include "URDFImporterInterface.h"
///BulletURDFImporter can deal with URDF and (soon) SDF files
class BulletURDFImporter : public URDFImporterInterface
{
@@ -18,6 +19,11 @@ public:
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
virtual int getNumModels() const;
virtual void activateModel(int modelIndex);
const char* getPathPrefix();
void printTree(); //for debugging

2
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -163,7 +163,7 @@ static btVector4 colors[4] =
};
btVector3 selectColor()
static btVector3 selectColor()
{
static int curColor = 0;

2
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -1,6 +1,6 @@
#include "MyMultiBodyCreator.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"

2
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -17,6 +17,8 @@ public:
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)=0;
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false;}
virtual const char* getPathPrefix()=0;
///return >=0 for the root link index, -1 if there is no root link

372
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -1,18 +1,33 @@
#include "UrdfParser.h"
#include "tinyxml/tinyxml.h"
#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "urdfStringSplit.h"
#include "urdfLexicalCast.h"
UrdfParser::UrdfParser()
:m_parseSDF(false),
m_activeSdfModel(-1)
{
}
UrdfParser::~UrdfParser()
{
cleanModel(&m_urdf2Model);
for (int i=0;i<m_model.m_materials.size();i++)
for (int i=0;i<m_tmpModels.size();i++)
{
UrdfMaterial** matPtr = m_model.m_materials.getAtIndex(i);
cleanModel(m_tmpModels[i]);
}
m_sdfModels.clear();
m_tmpModels.clear();
}
void UrdfParser::cleanModel(UrdfModel* model)
{
for (int i=0;i<model->m_materials.size();i++)
{
UrdfMaterial** matPtr = model->m_materials.getAtIndex(i);
if (matPtr)
{
UrdfMaterial* mat = *matPtr;
@@ -20,26 +35,25 @@ UrdfParser::~UrdfParser()
}
}
for (int i=0;i<m_model.m_links.size();i++)
for (int i=0;i<model->m_links.size();i++)
{
UrdfLink** linkPtr = m_model.m_links.getAtIndex(i);
UrdfLink** linkPtr = model->m_links.getAtIndex(i);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
delete link;
}
}
for (int i=0;i<m_model.m_joints.size();i++)
for (int i=0;i<model->m_joints.size();i++)
{
UrdfJoint** jointPtr = m_model.m_joints.getAtIndex(i);
UrdfJoint** jointPtr = model->m_joints.getAtIndex(i);
if (jointPtr)
{
UrdfJoint* joint = *jointPtr;
delete joint;
}
}
}
static bool parseVector4(btVector4& vec4, const std::string& vector_str)
@@ -192,14 +206,19 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
logger->reportError("Inertial element must have a mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
logger->reportError("Inertial: mass element must have value attribute");
return false;
}
if (m_parseSDF)
{
inertia.m_mass = urdfLexicalCast<double>(mass_xml->GetText());
} else
{
if (!mass_xml->Attribute("value"))
{
logger->reportError("Inertial: mass element must have value attribute");
return false;
}
inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
@@ -207,21 +226,45 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
logger->reportError("Inertial element must have inertia element");
return false;
}
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
if (m_parseSDF)
{
TiXmlElement* ixx = inertia_xml->FirstChildElement("ixx");
TiXmlElement* ixy = inertia_xml->FirstChildElement("ixy");
TiXmlElement* ixz = inertia_xml->FirstChildElement("ixz");
TiXmlElement* iyy = inertia_xml->FirstChildElement("iyy");
TiXmlElement* iyz = inertia_xml->FirstChildElement("iyz");
TiXmlElement* izz = inertia_xml->FirstChildElement("izz");
if (ixx && ixy && ixz && iyy && iyz && izz)
{
inertia.m_ixx = urdfLexicalCast<double>(ixx->GetText());
inertia.m_ixy = urdfLexicalCast<double>(ixy->GetText());
inertia.m_ixz = urdfLexicalCast<double>(ixz->GetText());
inertia.m_iyy = urdfLexicalCast<double>(iyy->GetText());
inertia.m_iyz = urdfLexicalCast<double>(iyz->GetText());
inertia.m_izz = urdfLexicalCast<double>(izz->GetText());
} else
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements");
return false;
}
} else
{
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
}
return true;
}
bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger)
@@ -251,13 +294,27 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
else if (type_name == "box")
{
geom.m_type = URDF_GEOM_BOX;
if (!shape->Attribute("size"))
{
logger->reportError("box requires a size attribute");
} else
{
parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
}
if (m_parseSDF)
{
TiXmlElement* size = shape->FirstChildElement("size");
if (0==size)
{
logger->reportError("box requires a size child element");
return false;
}
parseVector3(geom.m_boxSize,size->GetText(),logger);
}
else
{
if (!shape->Attribute("size"))
{
logger->reportError("box requires a size attribute");
return false;
} else
{
parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
}
}
}
else if (type_name == "cylinder")
{
@@ -294,9 +351,29 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
else
{
logger->reportError("Unknown geometry type:");
logger->reportError(type_name.c_str());
return false;
if (this->m_parseSDF)
{
if (type_name == "plane")
{
geom.m_type = URDF_GEOM_PLANE;
TiXmlElement *n = shape->FirstChildElement("normal");
TiXmlElement *s = shape->FirstChildElement("size");
if ((0==n)||(0==s))
{
logger->reportError("Plane shape must have both normal and size attributes");
return false;
}
parseVector3(geom.m_planeNormal,n->GetText(),logger);
}
} else
{
logger->reportError("Unknown geometry type:");
logger->reportError(type_name.c_str());
return false;
}
}
return true;
@@ -331,7 +408,7 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
return true;
}
bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
@@ -358,7 +435,8 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (mat)
//todo(erwincoumans) skip materials in SDF for now (due to complexity)
if (mat && !m_parseSDF)
{
// get material name
if (!mat->Attribute("name"))
@@ -377,7 +455,7 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
m_model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
}
}
@@ -386,7 +464,7 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
return true;
}
bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
@@ -435,7 +513,7 @@ bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* lo
{
UrdfVisual visual;
if (parseVisual(visual, vis_xml,logger))
if (parseVisual(model, visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
@@ -676,16 +754,16 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
}
bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
{
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
btHashMap<btHashString,btHashString> parentLinkTree;
// loop through all joints, for every link, assign children links and children joints
for (int i=0;i<m_model.m_joints.size();i++)
for (int i=0;i<model.m_joints.size();i++)
{
UrdfJoint** jointPtr = m_model.m_joints.getAtIndex(i);
UrdfJoint** jointPtr = model.m_joints.getAtIndex(i);
if (jointPtr)
{
UrdfJoint* joint = *jointPtr;
@@ -698,7 +776,7 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
return false;
}
UrdfLink** childLinkPtr = m_model.m_links.find(joint->m_childLinkName.c_str());
UrdfLink** childLinkPtr = model.m_links.find(joint->m_childLinkName.c_str());
if (!childLinkPtr)
{
logger->reportError("Cannot find child link for joint ");
@@ -708,7 +786,7 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
}
UrdfLink* childLink = *childLinkPtr;
UrdfLink** parentLinkPtr = m_model.m_links.find(joint->m_parentLinkName.c_str());
UrdfLink** parentLinkPtr = model.m_links.find(joint->m_parentLinkName.c_str());
if (!parentLinkPtr)
{
logger->reportError("Cannot find parent link for a joint");
@@ -728,9 +806,9 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
}
//search for children that have no parent, those are 'root'
for (int i=0;i<m_model.m_links.size();i++)
for (int i=0;i<model.m_links.size();i++)
{
UrdfLink** linkPtr = m_model.m_links.getAtIndex(i);
UrdfLink** linkPtr = model.m_links.getAtIndex(i);
btAssert(linkPtr);
if (linkPtr)
{
@@ -739,18 +817,18 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
if (!link->m_parentLink)
{
m_model.m_rootLinks.push_back(link);
model.m_rootLinks.push_back(link);
}
}
}
if (m_model.m_rootLinks.size()>1)
if (model.m_rootLinks.size()>1)
{
logger->reportWarning("URDF file with multiple root links found");
}
if (m_model.m_rootLinks.size()==0)
if (model.m_rootLinks.size()==0)
{
logger->reportError("URDF without root link found");
return false;
@@ -785,7 +863,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
logger->reportError("Expected a name for robot");
return false;
}
m_model.m_name = name;
m_urdf2Model.m_name = name;
@@ -797,13 +875,13 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
parseMaterial(*material, material_xml, logger);
UrdfMaterial** mat =m_model.m_materials.find(material->m_name.c_str());
UrdfMaterial** mat =m_urdf2Model.m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
} else
{
m_model.m_materials.insert(material->m_name.c_str(),material);
m_urdf2Model.m_materials.insert(material->m_name.c_str(),material);
}
}
@@ -817,9 +895,9 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
{
UrdfLink* link = new UrdfLink;
if (parseLink(*link, link_xml,logger))
if (parseLink(m_urdf2Model,*link, link_xml,logger))
{
if (m_model.m_links.find(link->m_name.c_str()))
if (m_urdf2Model.m_links.find(link->m_name.c_str()))
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
@@ -832,7 +910,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = m_model.m_materials.find(vis.m_materialName.c_str());
UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
@@ -844,7 +922,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
m_model.m_links.insert(link->m_name.c_str(),link);
m_urdf2Model.m_links.insert(link->m_name.c_str(),link);
}
} else
{
@@ -854,7 +932,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
if (m_model.m_links.size() == 0)
if (m_urdf2Model.m_links.size() == 0)
{
logger->reportWarning("No links found in URDF file");
return false;
@@ -867,7 +945,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
if (parseJoint(*joint, joint_xml,logger))
{
if (m_model.m_joints.find(joint->m_name.c_str()))
if (m_urdf2Model.m_joints.find(joint->m_name.c_str()))
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
@@ -875,7 +953,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
else
{
m_model.m_joints.insert(joint->m_name.c_str(),joint);
m_urdf2Model.m_joints.insert(joint->m_name.c_str(),joint);
}
}
else
@@ -885,7 +963,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
bool ok(initTreeAndRoot(logger));
bool ok(initTreeAndRoot(m_urdf2Model,logger));
if (!ok)
{
return false;
@@ -893,9 +971,9 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
if (forceFixedBase)
{
for (int i=0;i<m_model.m_rootLinks.size();i++)
for (int i=0;i<m_urdf2Model.m_rootLinks.size();i++)
{
UrdfLink* link(m_model.m_rootLinks.at(i));
UrdfLink* link(m_urdf2Model.m_rootLinks.at(i));
link->m_inertia.m_mass = 0.0;
link->m_inertia.m_ixx = 0.0;
link->m_inertia.m_ixy = 0.0;
@@ -908,3 +986,161 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
return true;
}
void UrdfParser::activateModel(int modelIndex)
{
m_activeSdfModel = modelIndex;
}
bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
{
TiXmlDocument xml_doc;
xml_doc.Parse(sdfText);
if (xml_doc.Error())
{
logger->reportError(xml_doc.ErrorDesc());
xml_doc.ClearError();
return false;
}
TiXmlElement *sdf_xml = xml_doc.FirstChildElement("sdf");
if (!sdf_xml)
{
logger->reportError("expected an sdf element");
return false;
}
TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
if (!world_xml)
{
logger->reportError("expected a world element");
return false;
}
// Get all model (robot) elements
for (TiXmlElement* robot_xml = world_xml->FirstChildElement("model"); robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
{
UrdfModel* localModel = new UrdfModel;
m_tmpModels.push_back(localModel);
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
logger->reportError("Expected a name for robot");
return false;
}
localModel->m_name = name;
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
UrdfMaterial* material = new UrdfMaterial;
parseMaterial(*material, material_xml, logger);
UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
} else
{
localModel->m_materials.insert(material->m_name.c_str(),material);
}
}
// char msg[1024];
// sprintf(msg,"Num materials=%d", m_model.m_materials.size());
// logger->printMessage(msg);
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
UrdfLink* link = new UrdfLink;
if (parseLink(*localModel, *link, link_xml,logger))
{
if (localModel->m_links.find(link->m_name.c_str()))
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
return false;
} else
{
//copy model material into link material, if link has no local material
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
} else
{
//logger->reportError("Cannot find material with name:");
//logger->reportError(vis.m_materialName.c_str());
}
}
}
localModel->m_links.insert(link->m_name.c_str(),link);
}
} else
{
logger->reportError("failed to parse link");
delete link;
return false;
}
}
if (localModel->m_links.size() == 0)
{
logger->reportWarning("No links found in URDF file");
return false;
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
UrdfJoint* joint = new UrdfJoint;
if (parseJoint(*joint, joint_xml,logger))
{
if (localModel->m_joints.find(joint->m_name.c_str()))
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
return false;
}
else
{
localModel->m_joints.insert(joint->m_name.c_str(),joint);
}
}
else
{
logger->reportError("joint xml is not initialized correctly");
return false;
}
}
bool ok(initTreeAndRoot(*localModel,logger));
if (!ok)
{
return false;
}
m_sdfModels.push_back(localModel);
}
return true;
}

78
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -42,7 +42,9 @@ enum UrdfGeomTypes
URDF_GEOM_SPHERE=2,
URDF_GEOM_BOX,
URDF_GEOM_CYLINDER,
URDF_GEOM_MESH
URDF_GEOM_MESH,
URDF_GEOM_PLANE,
};
@@ -57,6 +59,8 @@ struct UrdfGeometry
double m_cylinderRadius;
double m_cylinderLength;
btVector3 m_planeNormal;
std::string m_meshFileName;
btVector3 m_meshScale;
};
@@ -134,32 +138,88 @@ struct UrdfModel
class UrdfParser
{
protected:
UrdfModel m_urdf2Model;
btAlignedObjectArray<UrdfModel*> m_sdfModels;
btAlignedObjectArray<UrdfModel*> m_tmpModels;
bool m_parseSDF;
int m_activeSdfModel;
void cleanModel(UrdfModel* model);
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
bool parseVisual(UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
bool parseCollision(UrdfCollision& collision, class TiXmlElement* config, ErrorLogger* logger);
bool initTreeAndRoot(ErrorLogger* logger);
bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
bool parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
UrdfModel m_model;
bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
public:
UrdfParser();
virtual ~UrdfParser();
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
void setParseSDF(bool useSDF)
{
m_parseSDF = useSDF;
}
bool getParseSDF() const
{
return m_parseSDF;
}
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
bool loadSDF(const char* sdfText, ErrorLogger* logger);
int getNumModels() const
{
//user should have loaded an SDF when calling this method
btAssert(m_parseSDF);
if (m_parseSDF)
{
return m_sdfModels.size();
}
}
void activateModel(int modelIndex);
UrdfModel& getModelByIndex(int index)
{
//user should have loaded an SDF when calling this method
btAssert(m_parseSDF);
const UrdfModel& getModel() const
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
{
return m_model;
if (m_parseSDF)
{
return *m_sdfModels[m_activeSdfModel];
}
return m_urdf2Model;
}
UrdfModel& getModel()
{
return m_model;
if (m_parseSDF)
{
return *m_sdfModels[m_activeSdfModel];
}
return m_urdf2Model;
}
};