void ofxBvh::parseHierarchy(const string& data)
{
vector<string> tokens;
string token;
total_channels = 0;
num_frames = 0;
frame_time = 0;
for (int i = 0; i < data.size(); i++)
{
char c = data[i];
if (isspace(c))
{
if (!token.empty()) tokens.push_back(token);
token.clear();
}
else
{
token.push_back(c);
}
}
int index = 0;
while (index < tokens.size())
{
if (tokens[index++] == "ROOT")
{
root = parseJoint(index, tokens, NULL);
}
}
}
Skeleton BvhReader::getSkeleton() {
parseJoint("");
return _skeleton;
}
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;
TiXmlElement* pose_xml = robot_xml->FirstChildElement("pose");
if (0==pose_xml)
{
localModel->m_rootTransformInWorld.setIdentity();
}
else
{
parseTransform(localModel->m_rootTransformInWorld,pose_xml,logger,m_parseSDF);
}
// 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;
}
bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase)
{
TiXmlDocument xml_doc;
xml_doc.Parse(urdfText);
if (xml_doc.Error())
{
logger->reportError(xml_doc.ErrorDesc());
xml_doc.ClearError();
return false;
}
TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
logger->reportError("expected a robot element");
return false;
}
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
logger->reportError("Expected a name for robot");
return false;
}
m_urdf2Model.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 =m_urdf2Model.m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
} else
{
m_urdf2Model.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(m_urdf2Model,*link, link_xml,logger))
{
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());
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 = m_urdf2Model.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());
}
}
}
m_urdf2Model.m_links.insert(link->m_name.c_str(),link);
}
} else
{
logger->reportError("failed to parse link");
delete link;
return false;
}
}
if (m_urdf2Model.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 (m_urdf2Model.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
{
m_urdf2Model.m_joints.insert(joint->m_name.c_str(),joint);
}
}
else
{
logger->reportError("joint xml is not initialized correctly");
return false;
}
}
bool ok(initTreeAndRoot(m_urdf2Model,logger));
if (!ok)
{
return false;
}
if (forceFixedBase)
{
for (int i=0;i<m_urdf2Model.m_rootLinks.size();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;
link->m_inertia.m_ixz = 0.0;
link->m_inertia.m_iyy = 0.0;
link->m_inertia.m_iyz = 0.0;
link->m_inertia.m_izz = 0.0;
}
}
return true;
}
ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
{
ModelInterfaceSharedPtr model(new ModelInterface);
model->clear();
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
if (xml_doc.Error())
{
CONSOLE_BRIDGE_logError(xml_doc.ErrorDesc());
xml_doc.ClearError();
model.reset();
return model;
}
TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file");
model.reset();
return model;
}
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
CONSOLE_BRIDGE_logError("No name given for the robot.");
model.reset();
return model;
}
model->name_ = std::string(name);
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
MaterialSharedPtr material;
material.reset(new Material);
try {
parseMaterial(*material, material_xml, false); // material needs to be fully defined here
if (model->getMaterial(material->name))
{
CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str());
material.reset();
model.reset();
return model;
}
else
{
model->materials_.insert(make_pair(material->name,material));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str());
}
}
catch (ParseError &/*e*/) {
CONSOLE_BRIDGE_logError("material xml is not initialized correctly");
material.reset();
model.reset();
return model;
}
}
// Get all Link elements
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
LinkSharedPtr link;
link.reset(new Link);
try {
parseLink(*link, link_xml);
if (model->getLink(link->name))
{
CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str());
model.reset();
return model;
}
else
{
// set link visual(s) material
CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str());
if (link->visual)
{
assignMaterial(link->visual, model, link->name.c_str());
}
for (const auto& visual : link->visual_array)
{
assignMaterial(visual, model, link->name.c_str());
}
model->links_.insert(make_pair(link->name,link));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str());
}
}
catch (ParseError &/*e*/) {
CONSOLE_BRIDGE_logError("link xml is not initialized correctly");
model.reset();
return model;
}
}
if (model->links_.empty()){
CONSOLE_BRIDGE_logError("No link elements found in urdf file");
model.reset();
return model;
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
JointSharedPtr joint;
joint.reset(new Joint);
if (parseJoint(*joint, joint_xml))
{
if (model->getJoint(joint->name))
{
CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str());
model.reset();
return model;
}
else
{
model->joints_.insert(make_pair(joint->name,joint));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str());
}
}
else
{
CONSOLE_BRIDGE_logError("joint xml is not initialized correctly");
model.reset();
return model;
}
}
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
std::map<std::string, std::string> parent_link_tree;
parent_link_tree.clear();
// building tree: name mapping
try
{
model->initTree(parent_link_tree);
}
catch(ParseError &e)
{
CONSOLE_BRIDGE_logError("Failed to build tree: %s", e.what());
model.reset();
return model;
}
// find the root link
try
{
model->initRoot(parent_link_tree);
}
catch(ParseError &e)
{
CONSOLE_BRIDGE_logError("Failed to find root link: %s", e.what());
model.reset();
return model;
}
return model;
}
ofxBvhJoint* ofxBvh::parseJoint(int& index, vector<string> &tokens, ofxBvhJoint *parent)
{
string name = tokens[index++];
ofxBvhJoint *joint = new ofxBvhJoint(name, parent);
if (parent) parent->children.push_back(joint);
joint->bvh = this;
joints.push_back(joint);
jointMap[name] = joint;
while (index < tokens.size())
{
string token = tokens[index++];
if (token == "OFFSET")
{
joint->initial_offset.x = ofToFloat(tokens[index++]);
joint->initial_offset.y = ofToFloat(tokens[index++]);
joint->initial_offset.z = ofToFloat(tokens[index++]);
joint->offset = joint->initial_offset;
}
else if (token == "CHANNELS")
{
int num = ofToInt(tokens[index++]);
joint->channel_type.resize(num);
total_channels += num;
for (int i = 0; i < num; i++)
{
string ch = tokens[index++];
char axis = tolower(ch[0]);
char elem = tolower(ch[1]);
if (elem == 'p')
{
if (axis == 'x')
joint->channel_type[i] = ofxBvhJoint::X_POSITION;
else if (axis == 'y')
joint->channel_type[i] = ofxBvhJoint::Y_POSITION;
else if (axis == 'z')
joint->channel_type[i] = ofxBvhJoint::Z_POSITION;
else
{
ofLogError("ofxBvh", "invalid bvh format");
return NULL;
}
}
else if (elem == 'r')
{
if (axis == 'x')
joint->channel_type[i] = ofxBvhJoint::X_ROTATION;
else if (axis == 'y')
joint->channel_type[i] = ofxBvhJoint::Y_ROTATION;
else if (axis == 'z')
joint->channel_type[i] = ofxBvhJoint::Z_ROTATION;
else
{
ofLogError("ofxBvh", "invalid bvh format");
return NULL;
}
}
else
{
ofLogError("ofxBvh", "invalid bvh format");
return NULL;
}
}
}
else if (token == "JOINT"
|| token == "End")
{
parseJoint(index, tokens, joint);
}
else if (token == "}")
{
break;
}
}
return joint;
}
