srdf_writer.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Dave Coleman */

#include <tinyxml.h>
#include <srdfdom/srdf_writer.h>

namespace srdf
{
// ******************************************************************************************
// Constructor
// ******************************************************************************************
SRDFWriter::SRDFWriter()
{
  // Intialize the SRDF model
  srdf_model_.reset(new srdf::Model());
}

// ******************************************************************************************
// Destructor
// ******************************************************************************************
SRDFWriter::~SRDFWriter()
{
}

// ******************************************************************************************
// Load SRDF data from a pre-populated string
// ******************************************************************************************
bool SRDFWriter::initString(const urdf::ModelInterface& robot_model, const std::string& srdf_string)
{
  // Parse string into srdf_model_ and Error check
  if (!srdf_model_->initString(robot_model, srdf_string))
  {
    return false;  // error loading file. improper format?
  }

  // copy fields into this object
  initModel(robot_model, *srdf_model_);

  return true;
}

// ******************************************************************************************
// Load SRDF data from a pre-populated string
// ******************************************************************************************
void SRDFWriter::initModel(const urdf::ModelInterface& robot_model, const srdf::Model& srdf_model)
{
  // copy to internal srdf_model_
  if (srdf_model_.get() != &srdf_model)
  {
    *srdf_model_ = srdf_model;
  }

  // Copy all read-only data from srdf model to this object
  disabled_collisions_ = srdf_model_->getDisabledCollisionPairs();
  link_sphere_approximations_ = srdf_model_->getLinkSphereApproximations();
  groups_ = srdf_model_->getGroups();
  virtual_joints_ = srdf_model_->getVirtualJoints();
  end_effectors_ = srdf_model_->getEndEffectors();
  group_states_ = srdf_model_->getGroupStates();
  passive_joints_ = srdf_model_->getPassiveJoints();

  // Copy the robot name b/c the root xml element requires this attribute
  robot_name_ = robot_model.getName();
}

// ******************************************************************************************
// Update the SRDF Model class using a new SRDF string
// ******************************************************************************************
void SRDFWriter::updateSRDFModel(const urdf::ModelInterface& robot_model)
{
  // Get an up to date SRDF String
  const std::string srdf_string = getSRDFString();

  // Error check
  if (!srdf_model_->initString(robot_model, srdf_string))
  {
    throw std::runtime_error("Unable to update the SRDF Model");
  }
}

// ******************************************************************************************
// Save to file a generated SRDF document
// ******************************************************************************************
bool SRDFWriter::writeSRDF(const std::string& file_path)
{
  // Generate the SRDF
  TiXmlDocument document = generateSRDF();

  // Save to file
  return document.SaveFile(file_path);
}

// ******************************************************************************************
// Get a string of a generated SRDF document
// ******************************************************************************************
std::string SRDFWriter::getSRDFString()
{
  // Generate the SRDF
  TiXmlDocument document = generateSRDF();

  // Setup printer
  TiXmlPrinter printer;
  printer.SetIndent("    ");
  document.Accept(&printer);

  // Return string
  return printer.CStr();
}

// ******************************************************************************************
// Generate SRDF XML of all contained data
// ******************************************************************************************
TiXmlDocument SRDFWriter::generateSRDF()
{
  TiXmlDocument document;
  TiXmlDeclaration* decl = new TiXmlDeclaration("1.0", "", "");
  document.LinkEndChild(decl);

  // Convenience comments
  TiXmlComment* comment = new TiXmlComment("This does not replace URDF, and is not an extension of URDF.\n    This is "
                                           "a format for "
                                           "representing semantic information about the robot structure.\n    A URDF "
                                           "file must exist for "
                                           "this robot as well, where the joints and the links that are referenced are "
                                           "defined\n");
  document.LinkEndChild(comment);

  // Root
  TiXmlElement* robot_root = new TiXmlElement("robot");
  robot_root->SetAttribute("name", robot_name_);  // robot name
  document.LinkEndChild(robot_root);

  // Add Groups
  createGroupsXML(robot_root);

  // Add Group States
  createGroupStatesXML(robot_root);

  // Add End Effectors
  createEndEffectorsXML(robot_root);

  // Add Virtual Joints
  createVirtualJointsXML(robot_root);

  // Add Passive Joints
  createPassiveJointsXML(robot_root);

  // Add Link Sphere approximations
  createLinkSphereApproximationsXML(robot_root);

  // Add Disabled Collisions
  createDisabledCollisionsXML(robot_root);

  // Save
  return document;
}

// ******************************************************************************************
// Generate XML for SRDF groups
// ******************************************************************************************
void SRDFWriter::createGroupsXML(TiXmlElement* root)
{
  // Convenience comments
  if (groups_.size())  // only show comments if there are corresponding elements
  {
    TiXmlComment* comment;
    comment = new TiXmlComment("GROUPS: Representation of a set of joints and links. This can be useful for specifying "
                               "DOF to plan for, defining arms, end effectors, etc");
    root->LinkEndChild(comment);
    comment = new TiXmlComment("LINKS: When a link is specified, the parent joint of that link (if it exists) is "
                               "automatically included");
    root->LinkEndChild(comment);
    comment = new TiXmlComment("JOINTS: When a joint is specified, the child link of that joint (which will always "
                               "exist) is automatically included");
    root->LinkEndChild(comment);
    comment = new TiXmlComment("CHAINS: When a chain is specified, all the links along the chain (including endpoints) "
                               "are included in the group. Additionally, all the joints that are parents to included "
                               "links are also included. This means that joints along the chain and the parent joint "
                               "of the base link are included in the group");
    root->LinkEndChild(comment);
    comment = new TiXmlComment("SUBGROUPS: Groups can also be formed by referencing to already defined group names");
    root->LinkEndChild(comment);
  }

  // Loop through all of the top groups
  for (std::vector<srdf::Model::Group>::iterator group_it = groups_.begin(); group_it != groups_.end(); ++group_it)
  {
    // Create group element
    TiXmlElement* group = new TiXmlElement("group");
    group->SetAttribute("name", group_it->name_);  // group name
    root->LinkEndChild(group);

    // LINKS
    for (std::vector<std::string>::const_iterator link_it = group_it->links_.begin(); link_it != group_it->links_.end();
         ++link_it)
    {
      TiXmlElement* link = new TiXmlElement("link");
      link->SetAttribute("name", *link_it);  // link name
      group->LinkEndChild(link);
    }

    // JOINTS
    for (std::vector<std::string>::const_iterator joint_it = group_it->joints_.begin();
         joint_it != group_it->joints_.end(); ++joint_it)
    {
      TiXmlElement* joint = new TiXmlElement("joint");
      joint->SetAttribute("name", *joint_it);  // joint name
      group->LinkEndChild(joint);
    }

    // CHAINS
    for (std::vector<std::pair<std::string, std::string> >::const_iterator chain_it = group_it->chains_.begin();
         chain_it != group_it->chains_.end(); ++chain_it)
    {
      TiXmlElement* chain = new TiXmlElement("chain");
      chain->SetAttribute("base_link", chain_it->first);
      chain->SetAttribute("tip_link", chain_it->second);
      group->LinkEndChild(chain);
    }

    // SUBGROUPS
    for (std::vector<std::string>::const_iterator subgroup_it = group_it->subgroups_.begin();
         subgroup_it != group_it->subgroups_.end(); ++subgroup_it)
    {
      TiXmlElement* subgroup = new TiXmlElement("group");
      subgroup->SetAttribute("name", *subgroup_it);  // subgroup name
      group->LinkEndChild(subgroup);
    }
  }
}

// ******************************************************************************************
// Generate XML for SRDF link collision spheres
// ******************************************************************************************
void SRDFWriter::createLinkSphereApproximationsXML(TiXmlElement* root)
{
  if (link_sphere_approximations_.empty())  // skip it if there are none
    return;

  // Convenience comments
  TiXmlComment* comment = new TiXmlComment();
  comment->SetValue("COLLISION SPHERES: Purpose: Define a set of spheres that bounds a link.");
  root->LinkEndChild(comment);

  for (std::vector<srdf::Model::LinkSpheres>::const_iterator link_sphere_it = link_sphere_approximations_.begin();
       link_sphere_it != link_sphere_approximations_.end(); ++link_sphere_it)
  {
    if (link_sphere_it->spheres_.empty())  // skip if no spheres for this link
      continue;

    // Create new element for the link
    TiXmlElement* link = new TiXmlElement("link_sphere_approximation");
    link->SetAttribute("link", link_sphere_it->link_);
    root->LinkEndChild(link);

    // Add all spheres for the link
    for (std::vector<srdf::Model::Sphere>::const_iterator sphere_it = link_sphere_it->spheres_.begin();
         sphere_it != link_sphere_it->spheres_.end(); ++sphere_it)
    {
      TiXmlElement* sphere = new TiXmlElement("sphere");
      std::stringstream center;
      center.precision(20);
      center << sphere_it->center_x_ << " " << sphere_it->center_y_ << " " << sphere_it->center_z_;
      sphere->SetAttribute("center", center.str());
      sphere->SetDoubleAttribute("radius", sphere_it->radius_);
      link->LinkEndChild(sphere);
    }
  }
}

// ******************************************************************************************
// Generate XML for SRDF disabled collisions of robot link pairs
// ******************************************************************************************
void SRDFWriter::createDisabledCollisionsXML(TiXmlElement* root)
{
  // Convenience comments
  if (disabled_collisions_.size())  // only show comments if there are corresponding elements
  {
    TiXmlComment* comment = new TiXmlComment();
    comment->SetValue("DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come "
                      "into collision with any other link in the robot. This tag disables collision checking between a "
                      "specified pair of links. ");
    root->LinkEndChild(comment);
  }

  for (std::vector<srdf::Model::DisabledCollision>::const_iterator pair_it = disabled_collisions_.begin();
       pair_it != disabled_collisions_.end(); ++pair_it)
  {
    // Create new element for each link pair
    TiXmlElement* link_pair = new TiXmlElement("disable_collisions");
    link_pair->SetAttribute("link1", pair_it->link1_);
    link_pair->SetAttribute("link2", pair_it->link2_);
    link_pair->SetAttribute("reason", pair_it->reason_);

    root->LinkEndChild(link_pair);
  }
}

// ******************************************************************************************
// Generate XML for SRDF group states
// ******************************************************************************************
void SRDFWriter::createGroupStatesXML(TiXmlElement* root)
{
  // Convenience comments
  if (group_states_.size())  // only show comments if there are corresponding elements
  {
    TiXmlComment* comment = new TiXmlComment();
    comment->SetValue("GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. "
                      "This is useful to define states like 'folded arms'");
    root->LinkEndChild(comment);
  }

  for (std::vector<srdf::Model::GroupState>::const_iterator state_it = group_states_.begin();
       state_it != group_states_.end(); ++state_it)
  {
    // Create new element for each group state
    TiXmlElement* state = new TiXmlElement("group_state");
    state->SetAttribute("name", state_it->name_);
    state->SetAttribute("group", state_it->group_);
    root->LinkEndChild(state);

    // Add all joints
    for (std::map<std::string, std::vector<double> >::const_iterator value_it = state_it->joint_values_.begin();
         value_it != state_it->joint_values_.end(); ++value_it)
    {
      TiXmlElement* joint = new TiXmlElement("joint");
      joint->SetAttribute("name", value_it->first);             // joint name
      joint->SetDoubleAttribute("value", value_it->second[0]);  // joint value

      // TODO: use the vector to support multi-DOF joints
      state->LinkEndChild(joint);
    }
  }
}

// ******************************************************************************************
// Generate XML for SRDF end effectors
// ******************************************************************************************
void SRDFWriter::createEndEffectorsXML(TiXmlElement* root)
{
  // Convenience comments
  if (end_effectors_.size())  // only show comments if there are corresponding elements
  {
    TiXmlComment* comment = new TiXmlComment();
    comment->SetValue("END EFFECTOR: Purpose: Represent information about an end effector.");
    root->LinkEndChild(comment);
  }

  for (std::vector<srdf::Model::EndEffector>::const_iterator effector_it = end_effectors_.begin();
       effector_it != end_effectors_.end(); ++effector_it)
  {
    // Create new element for each link pair
    TiXmlElement* effector = new TiXmlElement("end_effector");
    effector->SetAttribute("name", effector_it->name_);
    effector->SetAttribute("parent_link", effector_it->parent_link_);
    effector->SetAttribute("group", effector_it->component_group_);
    if (!effector_it->parent_group_.empty())
      effector->SetAttribute("parent_group", effector_it->parent_group_);
    root->LinkEndChild(effector);
  }
}

// ******************************************************************************************
// Generate XML for SRDF virtual joints
// ******************************************************************************************
void SRDFWriter::createVirtualJointsXML(TiXmlElement* root)
{
  // Convenience comments
  if (virtual_joints_.size())  // only show comments if there are corresponding elements
  {
    TiXmlComment* comment = new TiXmlComment();
    comment->SetValue("VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an "
                      "external frame of reference (considered fixed with respect to the robot)");
    root->LinkEndChild(comment);
  }

  for (std::vector<srdf::Model::VirtualJoint>::const_iterator virtual_it = virtual_joints_.begin();
       virtual_it != virtual_joints_.end(); ++virtual_it)
  {
    // Create new element for each link pair
    TiXmlElement* virtual_joint = new TiXmlElement("virtual_joint");
    virtual_joint->SetAttribute("name", virtual_it->name_);
    virtual_joint->SetAttribute("type", virtual_it->type_);
    virtual_joint->SetAttribute("parent_frame", virtual_it->parent_frame_);
    virtual_joint->SetAttribute("child_link", virtual_it->child_link_);

    root->LinkEndChild(virtual_joint);
  }
}

void SRDFWriter::createPassiveJointsXML(TiXmlElement* root)
{
  if (passive_joints_.size())
  {
    TiXmlComment* comment = new TiXmlComment();
    comment->SetValue("PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated");
    root->LinkEndChild(comment);
  }
  for (std::vector<srdf::Model::PassiveJoint>::const_iterator p_it = passive_joints_.begin();
       p_it != passive_joints_.end(); ++p_it)
  {
    // Create new element for each link pair
    TiXmlElement* p_joint = new TiXmlElement("passive_joint");
    p_joint->SetAttribute("name", p_it->name_);
    root->LinkEndChild(p_joint);
  }
}
}  // namespace srdf