/*
 * Copyright 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#include <fstream>
#include <sstream>
#include <algorithm>
#include <memory>
#include <string>
#include <set>
#include <ignition/math.hh>
#include <boost/algorithm/string.hpp>

#include "urdf_model/model.h"
#include "urdf_model/link.h"
#include "urdf_parser/urdf_parser.h"

#include "sdf/SDFExtension.hh"
#include "sdf/parser_urdf.hh"
#include "sdf/sdf.hh"

using namespace sdf;

typedef std::shared_ptr<urdf::Collision> UrdfCollisionPtr;
typedef std::shared_ptr<urdf::Visual> UrdfVisualPtr;
typedef std::shared_ptr<urdf::Link> UrdfLinkPtr;
typedef std::shared_ptr<const urdf::Link> ConstUrdfLinkPtr;
typedef std::shared_ptr<TiXmlElement> TiXmlElementPtr;
typedef std::shared_ptr<SDFExtension> SDFExtensionPtr;
typedef std::map<std::string, std::vector<SDFExtensionPtr> >
  StringSDFExtensionPtrMap;

/// create SDF geometry block based on URDF
StringSDFExtensionPtrMap g_extensions;
bool g_reduceFixedJoints;
bool g_enforceLimits;
std::string g_collisionExt = "_collision";
std::string g_visualExt = "_visual";
std::string g_lumpPrefix = "_fixed_joint_lump__";
urdf::Pose g_initialRobotPose;
bool g_initialRobotPoseValid = false;
std::set<std::string> g_fixedJointsNotReduced;

/// \brief parser xml string into urdf::Vector3
/// \param[in] _key XML key where vector3 value might be
/// \param[in] _scale scalar scale for the vector3
/// \return a urdf::Vector3
urdf::Vector3 ParseVector3(TiXmlNode* _key, double _scale = 1.0);
urdf::Vector3 ParseVector3(const std::string &_str, double _scale = 1.0);

/// insert extensions into collision geoms
void InsertSDFExtensionCollision(TiXmlElement *_elem,
    const std::string &_linkName);

/// insert extensions into model
void InsertSDFExtensionRobot(TiXmlElement *_elem);

/// insert extensions into visuals
void InsertSDFExtensionVisual(TiXmlElement *_elem,
    const std::string &_linkName);


/// insert extensions into joints
void InsertSDFExtensionJoint(TiXmlElement *_elem,
    const std::string &_jointName);

/// reduced fixed joints:  check if a fixed joint should be lumped
///   checking both the joint type and if disabledFixedJointLumping
///   option is set
bool FixedJointShouldBeReduced(std::shared_ptr<urdf::Joint> _jnt);

/// reduced fixed joints:  apply transform reduction for ray sensors
///   in extensions when doing fixed joint reduction
void ReduceSDFExtensionSensorTransformReduction(
      std::vector<TiXmlElementPtr>::iterator _blobIt,
      ignition::math::Pose3d _reductionTransform);

/// reduced fixed joints:  apply transform reduction for projectors in
///   extensions when doing fixed joint reduction
void ReduceSDFExtensionProjectorTransformReduction(
      std::vector<TiXmlElementPtr>::iterator _blobIt,
      ignition::math::Pose3d _reductionTransform);


/// reduced fixed joints:  apply transform reduction to extensions
///   when doing fixed joint reduction
void ReduceSDFExtensionsTransform(SDFExtensionPtr _ge);

/// reduce fixed joints:  lump joints to parent link
void ReduceJointsToParent(UrdfLinkPtr _link);

/// reduce fixed joints:  lump collisions to parent link
void ReduceCollisionsToParent(UrdfLinkPtr _link);

/// reduce fixed joints:  lump visuals to parent link
void ReduceVisualsToParent(UrdfLinkPtr _link);

/// reduce fixed joints:  lump inertial to parent link
void ReduceInertialToParent(UrdfLinkPtr /*_link*/);

/// create SDF Collision block based on URDF
void CreateCollision(TiXmlElement* _elem, ConstUrdfLinkPtr _link,
      UrdfCollisionPtr _collision,
      const std::string &_oldLinkName = std::string(""));

/// create SDF Visual block based on URDF
void CreateVisual(TiXmlElement *_elem, ConstUrdfLinkPtr _link,
      UrdfVisualPtr _visual, const std::string &_oldLinkName = std::string(""));

/// create SDF Joint block based on URDF
void CreateJoint(TiXmlElement *_root, ConstUrdfLinkPtr _link,
      ignition::math::Pose3d &_currentTransform);

/// insert extensions into links
void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName);

/// create visual blocks from urdf visuals
void CreateVisuals(TiXmlElement* _elem, ConstUrdfLinkPtr _link);

/// create collision blocks from urdf collisions
void CreateCollisions(TiXmlElement* _elem, ConstUrdfLinkPtr _link);

/// create SDF Inertial block based on URDF
void CreateInertial(TiXmlElement *_elem, ConstUrdfLinkPtr _link);

/// append transform (pose) to the end of the xml element
void AddTransform(TiXmlElement *_elem, const ignition::math::Pose3d &_transform);

/// create SDF from URDF link
void CreateSDF(TiXmlElement *_root, ConstUrdfLinkPtr _link,
      const ignition::math::Pose3d &_transform);

/// create SDF Link block based on URDF
void CreateLink(TiXmlElement *_root, ConstUrdfLinkPtr _link,
      ignition::math::Pose3d &_currentTransform);


/// print collision groups for debugging purposes
void PrintCollisionGroups(UrdfLinkPtr _link);

/// reduced fixed joints:  apply appropriate frame updates in joint
///   inside urdf extensions when doing fixed joint reduction
void ReduceSDFExtensionJointFrameReplace(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    UrdfLinkPtr _link);

/// reduced fixed joints:  apply appropriate frame updates in gripper
///   inside urdf extensions when doing fixed joint reduction
void ReduceSDFExtensionGripperFrameReplace(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    UrdfLinkPtr _link);

/// reduced fixed joints:  apply appropriate frame updates in projector
/// inside urdf extensions when doing fixed joint reduction
void ReduceSDFExtensionProjectorFrameReplace(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    UrdfLinkPtr _link);

/// reduced fixed joints:  apply appropriate frame updates in plugins
///   inside urdf extensions when doing fixed joint reduction
void ReduceSDFExtensionPluginFrameReplace(
      std::vector<TiXmlElementPtr>::iterator _blobIt,
      UrdfLinkPtr _link, const std::string &_pluginName,
      const std::string &_elementName,
      ignition::math::Pose3d _reductionTransform);

/// reduced fixed joints:  apply appropriate frame updates in urdf
///   extensions when doing fixed joint reduction
void ReduceSDFExtensionContactSensorFrameReplace(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    UrdfLinkPtr _link);

/// \brief reduced fixed joints:  apply appropriate updates to urdf
///   extensions when doing fixed joint reduction
///
/// Take the link's existing list of gazebo extensions, transfer them
/// into parent link.  Along the way, update local transforms by adding
/// the additional transform to parent.  Also, look through all
/// referenced link names with plugins and update references to current
/// link to the parent link. (ReduceSDFExtensionFrameReplace())
///
/// \param[in] _link pointer to urdf link, its extensions will be reduced
void ReduceSDFExtensionToParent(UrdfLinkPtr _link);

/// reduced fixed joints:  apply appropriate frame updates
///   in urdf extensions when doing fixed joint reduction
void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge, UrdfLinkPtr _link);


/// get value from <key value="..."/> pair and return it as string
std::string GetKeyValueAsString(TiXmlElement* _elem);


/// \brief append key value pair to the end of the xml element
/// \param[in] _elem pointer to xml element
/// \param[in] _key string containing key to add to xml element
/// \param[in] _value string containing value for the key added
void AddKeyValue(TiXmlElement *_elem, const std::string &_key,
                     const std::string &_value);

/// \brief convert values to string
/// \param[in] _count number of values in _values array
/// \param[in] _values array of double values
/// \return a string
std::string Values2str(unsigned int _count, const double *_values);


void CreateGeometry(TiXmlElement* _elem,
    std::shared_ptr<urdf::Geometry> _geometry);

std::string GetGeometryBoundingBox(std::shared_ptr<urdf::Geometry> _geometry,
    double *_sizeVals);

ignition::math::Pose3d inverseTransformToParentFrame(
    ignition::math::Pose3d _transformInLinkFrame,
    urdf::Pose _parentToLinkTransform);

/// reduced fixed joints: transform to parent frame
urdf::Pose TransformToParentFrame(urdf::Pose _transformInLinkFrame,
    urdf::Pose _parentToLinkTransform);

/// reduced fixed joints: transform to parent frame
ignition::math::Pose3d TransformToParentFrame(
    ignition::math::Pose3d _transformInLinkFrame,
    urdf::Pose _parentToLinkTransform);

/// reduced fixed joints: transform to parent frame
ignition::math::Pose3d TransformToParentFrame(
    ignition::math::Pose3d _transformInLinkFrame,
    ignition::math::Pose3d _parentToLinkTransform);

/// reduced fixed joints: utility to copy between urdf::Pose and
///   math::Pose
ignition::math::Pose3d CopyPose(urdf::Pose _pose);

/// reduced fixed joints: utility to copy between urdf::Pose and
///   math::Pose
urdf::Pose CopyPose(ignition::math::Pose3d _pose);

/////////////////////////////////////////////////
urdf::Vector3 ParseVector3(const std::string &_str, double _scale)
{
  std::vector<std::string> pieces;
  std::vector<double> vals;

  boost::split(pieces, _str, boost::is_any_of(" "));
  for (unsigned int i = 0; i < pieces.size(); ++i)
  {
    if (pieces[i] != "")
    {
      try
      {
        vals.push_back(_scale
            * boost::lexical_cast<double>(pieces[i].c_str()));
      }
      catch(boost::bad_lexical_cast &)
      {
        sdferr << "xml key [" << _str
          << "][" << i << "] value [" << pieces[i]
          << "] is not a valid double from a 3-tuple\n";
        return urdf::Vector3(0, 0, 0);
      }
    }
  }

  if (vals.size() == 3)
    return urdf::Vector3(vals[0], vals[1], vals[2]);
  else
    return urdf::Vector3(0, 0, 0);
}

/////////////////////////////////////////////////
urdf::Vector3 ParseVector3(TiXmlNode *_key, double _scale)
{
  if (_key != NULL)
  {
    TiXmlElement *key = _key->ToElement();
    if (key != NULL)
    {
      return ParseVector3(GetKeyValueAsString(key), _scale);
    }
    sdferr << "key[" << _key->Value() << "] does not contain a Vector3\n";
  }
  else
  {
    sdferr << "Pointer to XML node _key is NULL\n";
  }

  return urdf::Vector3(0, 0, 0);
}

/////////////////////////////////////////////////
/// \brief convert Vector3 to string
/// \param[in] _vector a urdf::Vector3
/// \return a string
std::string Vector32Str(const urdf::Vector3 _vector)
{
  std::stringstream ss;
  ss << _vector.x;
  ss << " ";
  ss << _vector.y;
  ss << " ";
  ss << _vector.z;
  return ss.str();
}

////////////////////////////////////////////////////////////////////////////////
/// \brief Check and add collision to parent link
/// \param[in] _parentLink destination for _collision
/// \param[in] _name urdfdom 0.3+: urdf collision group name with lumped
///            collision info (see ReduceCollisionsToParent).
///            urdfdom 0.2: collision name with lumped
///            collision info (see ReduceCollisionsToParent).
/// \param[in] _collision move this collision to _parentLink
////////////////////////////////////////////////////////////////
// IMPORTANT NOTE: URDF_GE_OP3
// IMPORTANT NOTE: on change from urdfdom_headers 0.2.x to 0.3.x
////////////////////////////////////////////////////////////////
// In urdfdom_headers 0.2.x, there are group names for
// visuals and collisions in Link class:
//   std::map<std::string,
//     std::shared_ptr<std::vector<std::shared_ptr<Visual> > >
//     > visual_groups;
//   std::map<std::string,
//     std::shared_ptr<std::vector<std::shared_ptr<Collision> > >
//     > collision_groups;
// and we have Visual::group_name and
//             Collision::group_name
// In urdfdom_headers 0.3.x,
//   - Link::visual_groups and Link::collision_groups are removed
//   - method Link::getVisuals(group_name) has been removed
//   - method Link::getCollisions(group_name) has been removed
//   - Visual::group_name renamed to Visual::name
//   - Collision::group_name renamed to Collision::name
////////////////////////////////////////////////////////////////
void ReduceCollisionToParent(UrdfLinkPtr _parentLink,
    const std::string &_name,
    UrdfCollisionPtr _collision)
{
#ifndef URDF_GE_0P3
  std::shared_ptr<std::vector<UrdfCollisionPtr> > cols;
  cols = _parentLink->getCollisions(_name);

  if (!cols)
  {
    // group does not exist, create one and add to map
    cols.reset(new std::vector<UrdfCollisionPtr>);
    // new group name, create add vector to map and add Collision to the vector
    _parentLink->collision_groups.insert(make_pair(_name, cols));
  }

  // group exists, add Collision to the vector in the map
  std::vector<UrdfCollisionPtr>::iterator colIt =
    find(cols->begin(), cols->end(), _collision);
  if (colIt != cols->end())
    sdfwarn << "attempted to add collision to link ["
      << _parentLink->name
      << "], but it already exists under group ["
      << _name << "]\n";
  else
    cols->push_back(_collision);
#else
  // added a check to see if _collision already exist in
  // _parentLink::collision_array if not, add it.
  _collision->name = _name;
  std::vector<UrdfCollisionPtr>::iterator collisionIt =
    find(_parentLink->collision_array.begin(),
         _parentLink->collision_array.end(),
         _collision);
  if (collisionIt != _parentLink->collision_array.end())
  {
    sdfwarn << "attempted to add collision [" << _collision->name
            << "] to link ["
            << _parentLink->name
            << "], but it already exists in collision_array under name ["
            << (*collisionIt)->name << "]\n";
  }
  else
  {
    _parentLink->collision_array.push_back(_collision);
  }
#endif
}

////////////////////////////////////////////////////////////////////////////////
/// \brief Check and add visual to parent link
/// \param[in] _parentLink destination for _visual
/// \param[in] _name urdfdom 0.3+: urdf visual group name with lumped
///            visual info (see ReduceVisualsToParent).
///            urdfdom 0.2: visual name with lumped
///            visual info (see ReduceVisualsToParent).
/// \param[in] _visual move this visual to _parentLink
////////////////////////////////////////////////////////////////
// IMPORTANT NOTE: URDF_GE_OP3
// IMPORTANT NOTE: on change from urdfdom_headers 0.2.x to 0.3.x
////////////////////////////////////////////////////////////////
// In urdfdom_headers 0.2.x, there are group names for
// visuals and collisions in Link class:
//   std::map<std::string,
//     std::shared_ptr<std::vector<std::shared_ptr<Visual> > >
//     > visual_groups;
//   std::map<std::string,
//     std::shared_ptr<std::vector<std::shared_ptr<Collision> > >
//     > collision_groups;
// and we have Visual::group_name and
//             Collision::group_name
// In urdfdom_headers 0.3.x,
//   - Link::visual_groups and Link::collision_groups are removed
//   - method Link::getVisuals(group_name) has been removed
//   - method Link::getCollisions(group_name) has been removed
//   - Visual::group_name renamed to Visual::name
//   - Collision::group_name renamed to Collision::name
////////////////////////////////////////////////////////////////
void ReduceVisualToParent(UrdfLinkPtr _parentLink,
    const std::string &_name,
    UrdfVisualPtr _visual)
{
#ifndef URDF_GE_0P3
  std::shared_ptr<std::vector<UrdfVisualPtr> > viss;
  viss = _parentLink->getVisuals(_name);

  if (!viss)
  {
    // group does not exist, create one and add to map
    viss.reset(new std::vector<UrdfVisualPtr>);
    // new group name, create vector, add vector to map and
    //   add Visual to the vector
    _parentLink->visual_groups.insert(make_pair(_name, viss));
    sdfdbg << "successfully added a new visual group name ["
          << _name << "]\n";
  }

  // group exists, add Visual to the vector in the map if it's not there
  std::vector<UrdfVisualPtr>::iterator visIt
    = find(viss->begin(), viss->end(), _visual);
  if (visIt != viss->end())
    sdfwarn << "attempted to add visual to link ["
      << _parentLink->name
      << "], but it already exists under group ["
      << _name << "]\n";
  else
    viss->push_back(_visual);
#else
  // added a check to see if _visual already exist in
  // _parentLink::visual_array if not, add it.
  _visual->name = _name;
  std::vector<UrdfVisualPtr>::iterator visualIt =
    find(_parentLink->visual_array.begin(),
         _parentLink->visual_array.end(),
         _visual);
  if (visualIt != _parentLink->visual_array.end())
  {
    sdfwarn << "attempted to add visual [" << _visual->name
      << "] to link ["
      << _parentLink->name
      << "], but it already exists in visual_array under name ["
      << (*visualIt)->name << "]\n";
  }
  else
  {
    _parentLink->visual_array.push_back(_visual);
  }
#endif
}

////////////////////////////////////////////////////////////////////////////////
/// reduce fixed joints by lumping inertial, visual and
// collision elements of the child link into the parent link
void ReduceFixedJoints(TiXmlElement *_root, UrdfLinkPtr _link)
{
  // if child is attached to self by fixed _link first go up the tree,
  //   check it's children recursively
  for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i)
    if (FixedJointShouldBeReduced(_link->child_links[i]->parent_joint))
      ReduceFixedJoints(_root, _link->child_links[i]);

  // reduce this _link's stuff up the tree to parent but skip first joint
  //   if it's the world
  if (_link->getParent() && _link->getParent()->name != "world" &&
      _link->parent_joint && FixedJointShouldBeReduced(_link->parent_joint) )
  {
    sdfdbg << "Fixed Joint Reduction: extension lumping from ["
           << _link->name << "] to [" << _link->getParent()->name << "]\n";

    // lump sdf extensions to parent, (give them new reference _link names)
    ReduceSDFExtensionToParent(_link);

    // reduce _link elements to parent
    ReduceInertialToParent(_link);
    ReduceVisualsToParent(_link);
    ReduceCollisionsToParent(_link);
    ReduceJointsToParent(_link);
  }

  // continue down the tree for non-fixed joints
  for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i)
    if (!FixedJointShouldBeReduced(_link->child_links[i]->parent_joint))
      ReduceFixedJoints(_root, _link->child_links[i]);
}

// ODE dMatrix
typedef double dMatrix3[4*3];
typedef double dVector3[4];
#define _R(i,j) R[(i)*4+(j)]
#define _I(i,j) I[(i)*4+(j)]
#define dRecip(x) ((1.0f/(x)))

struct dMass;
void dMassSetZero (dMass *m);
void dMassSetParameters (dMass *, double themass,
    double cgx, double cgy, double cgz,
    double I11, double I22, double I33,
    double I12, double I13, double I23);

struct dMass
{
  double mass;
  dVector3 c;
  dMatrix3 I;

  dMass()
  { dMassSetZero (this); }

  void setZero()
  { dMassSetZero (this); }

  void setParameters (double themass, double cgx, double cgy, double cgz,
      double I11, double I22, double I33,
      double I12, double I13, double I23)
  { dMassSetParameters (this,themass,cgx,cgy,cgz,I11,I22,I33,I12,I13,I23); }
};

void dSetZero (double *a, int n)
{
  // dAASSERT (a && n >= 0);
  double *acurr = a;
  int ncurr = n;
  while (ncurr > 0)
  {
    *(acurr++) = 0;
    --ncurr;
  }
}

void dMassSetZero (dMass *m)
{
  // dAASSERT (m);
  m->mass = 0.0;
  dSetZero(m->c, sizeof(m->c) / sizeof(double));
  dSetZero(m->I, sizeof(m->I) / sizeof(double));
}


void dMassSetParameters (dMass *m, double themass,
    double cgx, double cgy, double cgz,
    double I11, double I22, double I33,
    double I12, double I13, double I23)
{
  // dAASSERT (m);
  dMassSetZero (m);
  m->mass = themass;
  m->c[0] = cgx;
  m->c[1] = cgy;
  m->c[2] = cgz;
  m->_I(0,0) = I11;
  m->_I(1,1) = I22;
  m->_I(2,2) = I33;
  m->_I(0,1) = I12;
  m->_I(0,2) = I13;
  m->_I(1,2) = I23;
  m->_I(1,0) = I12;
  m->_I(2,0) = I13;
  m->_I(2,1) = I23;
  // dMassCheck (m);
}

void dRFromEulerAngles (dMatrix3 R, double phi, double theta, double psi)
{
  double sphi,cphi,stheta,ctheta,spsi,cpsi;
  // dAASSERT (R);
  sphi = sin(phi);
  cphi = cos(phi);
  stheta = sin(theta);
  ctheta = cos(theta);
  spsi = sin(psi);
  cpsi = cos(psi);
  _R(0,0) = cpsi*ctheta;
  _R(0,1) = spsi*ctheta;
  _R(0,2) =-stheta;
  _R(0,3) = 0.0;
  _R(1,0) = cpsi*stheta*sphi - spsi*cphi;
  _R(1,1) = spsi*stheta*sphi + cpsi*cphi;
  _R(1,2) = ctheta*sphi;
  _R(1,3) = 0.0;
  _R(2,0) = cpsi*stheta*cphi + spsi*sphi;
  _R(2,1) = spsi*stheta*cphi - cpsi*sphi;
  _R(2,2) = ctheta*cphi;
  _R(2,3) = 0.0;
}

double _dCalcVectorDot3(const double *a, const double *b, unsigned step_a,
unsigned step_b)
{
  return a[0] * b[0] + a[step_a] * b[step_b] + a[2 * step_a] * b[2 * step_b];
}

double dCalcVectorDot3 (const double *a, const double *b)
{
  return _dCalcVectorDot3(a,b,1,1);
}

double dCalcVectorDot3_41 (const double *a, const double *b)
{
  return _dCalcVectorDot3(a,b,4,1);
}

void dMultiply0_331(double *res, const double *a, const double *b)
{
  double res_0, res_1, res_2;
  res_0 = dCalcVectorDot3(a, b);
  res_1 = dCalcVectorDot3(a + 4, b);
  res_2 = dCalcVectorDot3(a + 8, b);
  res[0] = res_0; res[1] = res_1; res[2] = res_2;
}

void dMultiply1_331(double *res, const double *a, const double *b)
{
  double res_0, res_1, res_2;
  res_0 = dCalcVectorDot3_41(a, b);
  res_1 = dCalcVectorDot3_41(a + 1, b);
  res_2 = dCalcVectorDot3_41(a + 2, b);
  res[0] = res_0; res[1] = res_1; res[2] = res_2;
}

void dMultiply0_133(double *res, const double *a, const double *b)
{
  dMultiply1_331(res, b, a);
}

void dMultiply0_333(double *res, const double *a, const double *b)
{
  dMultiply0_133(res + 0, a + 0, b);
  dMultiply0_133(res + 4, a + 4, b);
  dMultiply0_133(res + 8, a + 8, b);
}

void dMultiply2_333(double *res, const double *a, const double *b)
{
  dMultiply0_331(res + 0, b, a + 0);
  dMultiply0_331(res + 4, b, a + 4);
  dMultiply0_331(res + 8, b, a + 8);
}

void dMassRotate (dMass *m, const dMatrix3 R)
{
  // if the body is rotated by `R' relative to its point of reference,
  // the new inertia about the point of reference is:
  //
  //   R * I * R'
  //
  // where I is the old inertia.

  dMatrix3 t1;
  double t2[3];

  // dAASSERT (m);

  // rotate inertia matrix
  dMultiply2_333 (t1,m->I,R);
  dMultiply0_333 (m->I,R,t1);

  // ensure perfect symmetry
  m->_I(1,0) = m->_I(0,1);
  m->_I(2,0) = m->_I(0,2);
  m->_I(2,1) = m->_I(1,2);

  // rotate center of mass
  dMultiply0_331 (t2,R,m->c);
  m->c[0] = t2[0];
  m->c[1] = t2[1];
  m->c[2] = t2[2];
}

void dSetCrossMatrixPlus(double *res, const double *a, unsigned skip)
{
  const double a_0 = a[0], a_1 = a[1], a_2 = a[2];
  res[1] = -a_2;
  res[2] = +a_1;
  res[skip+0] = +a_2;
  res[skip+2] = -a_0;
  res[2*skip+0] = -a_1;
  res[2*skip+1] = +a_0;
}

void dMassTranslate (dMass *m, double x, double y, double z)
{
  // if the body is translated by `a' relative to its point of reference,
  // the new inertia about the point of reference is:
  //
  //   I + mass*(crossmat(c)^2 - crossmat(c+a)^2)
  //
  // where c is the existing center of mass and I is the old inertia.

  int i,j;
  dMatrix3 ahat,chat,t1,t2;
  double a[3];

  // dAASSERT (m);

  // adjust inertia matrix
  dSetZero (chat,12);
  dSetCrossMatrixPlus (chat,m->c,4);
  a[0] = x + m->c[0];
  a[1] = y + m->c[1];
  a[2] = z + m->c[2];
  dSetZero (ahat,12);
  dSetCrossMatrixPlus (ahat,a,4);
  dMultiply0_333 (t1,ahat,ahat);
  dMultiply0_333 (t2,chat,chat);
  for (i=0; i<3; i++) for (j=0; j<3; j++)
    m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]);

  // ensure perfect symmetry
  m->_I(1,0) = m->_I(0,1);
  m->_I(2,0) = m->_I(0,2);
  m->_I(2,1) = m->_I(1,2);

  // adjust center of mass
  m->c[0] += x;
  m->c[1] += y;
  m->c[2] += z;
}

void dMassAdd (dMass *a, const dMass *b)
{
  int i;
  double denom = dRecip (a->mass + b->mass);
  for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom;
  a->mass += b->mass;
  for (i=0; i<12; i++) a->I[i] += b->I[i];
}

/////////////////////////////////////////////////
/// print mass for link for debugging
void PrintMass(const std::string &_linkName, const dMass &_mass)
{
  sdfdbg << "LINK NAME: [" << _linkName << "] from dMass\n";
  sdfdbg << "     MASS: [" << _mass.mass << "]\n";
  sdfdbg << "       CG: [" << _mass.c[0] << ", " << _mass.c[1] << ", "
  << _mass.c[2] << "]\n";
  sdfdbg << "        I: [" << _mass.I[0] << ", " << _mass.I[1] << ", "
  << _mass.I[2] << "]\n";
  sdfdbg << "           [" << _mass.I[4] << ", " << _mass.I[5] << ", "
  << _mass.I[6] << "]\n";
  sdfdbg << "           [" << _mass.I[8] << ", " << _mass.I[9] << ", "
  << _mass.I[10] << "]\n";
}

/////////////////////////////////////////////////
/// print mass for link for debugging
void PrintMass(const UrdfLinkPtr _link)
{
  sdfdbg << "LINK NAME: [" << _link->name << "] from dMass\n";
  sdfdbg << "     MASS: [" << _link->inertial->mass << "]\n";
  sdfdbg << "       CG: [" << _link->inertial->origin.position.x << ", "
    << _link->inertial->origin.position.y << ", "
    << _link->inertial->origin.position.z << "]\n";
  sdfdbg << "        I: [" << _link->inertial->ixx << ", "
    << _link->inertial->ixy << ", "
    << _link->inertial->ixz << "]\n";
  sdfdbg << "           [" << _link->inertial->ixy << ", "
    << _link->inertial->iyy << ", "
    << _link->inertial->iyz << "]\n";
  sdfdbg << "           [" << _link->inertial->ixz << ", "
    << _link->inertial->iyz << ", "
    << _link->inertial->izz << "]\n";
}

/////////////////////////////////////////////////
/// reduce fixed joints:  lump inertial to parent link
void ReduceInertialToParent(UrdfLinkPtr _link)
{
  // now lump all contents of this _link to parent
  if (_link->inertial)
  {
    dMatrix3 R;
    double phi, theta, psi;

    // get parent mass (in parent link cg frame)
    dMass parentMass;

    if (!_link->getParent()->inertial)
      _link->getParent()->inertial.reset(new urdf::Inertial);

    dMassSetParameters(&parentMass, _link->getParent()->inertial->mass,
        0, 0, 0,
        _link->getParent()->inertial->ixx, _link->getParent()->inertial->iyy,
        _link->getParent()->inertial->izz, _link->getParent()->inertial->ixy,
        _link->getParent()->inertial->ixz, _link->getParent()->inertial->iyz);

    // transform parent inertia to parent link origin
    _link->getParent()->inertial->origin.rotation.getRPY(phi, theta, psi);
    dRFromEulerAngles(R, -phi,      0,    0);
    dMassRotate(&parentMass, R);
    dRFromEulerAngles(R,    0, -theta,    0);
    dMassRotate(&parentMass, R);
    dRFromEulerAngles(R,    0,      0, -psi);
    dMassRotate(&parentMass, R);

    // un-translate link mass from cg(inertial frame) into link frame
    dMassTranslate(&parentMass,
        _link->getParent()->inertial->origin.position.x,
        _link->getParent()->inertial->origin.position.y,
        _link->getParent()->inertial->origin.position.z);

    PrintMass("parent: " + _link->getParent()->name, parentMass);
    // PrintMass(_link->getParent());

    //////////////////////////////////////////////
    //                                          //
    // create a_link mass (in _link's cg frame) //
    //                                          //
    //////////////////////////////////////////////
    dMass linkMass;
    dMassSetParameters(&linkMass, _link->inertial->mass,
        0, 0, 0,
        _link->inertial->ixx, _link->inertial->iyy, _link->inertial->izz,
        _link->inertial->ixy, _link->inertial->ixz, _link->inertial->iyz);

    PrintMass("link : " + _link->name, linkMass);

    ////////////////////////////////////////////
    //                                        //
    // from cg (inertial frame) to link frame //
    //                                        //
    ////////////////////////////////////////////

    // Un-rotate _link mass from cg(inertial frame) into link frame
    _link->inertial->origin.rotation.getRPY(phi, theta, psi);
    dRFromEulerAngles(R, -phi,      0,    0);
    dMassRotate(&linkMass, R);
    dRFromEulerAngles(R,    0, -theta,    0);
    dMassRotate(&linkMass, R);
    dRFromEulerAngles(R,    0,      0, -psi);
    dMassRotate(&linkMass, R);

    // un-translate link mass from cg(inertial frame) into link frame
    dMassTranslate(&linkMass,
        _link->inertial->origin.position.x,
        _link->inertial->origin.position.y,
        _link->inertial->origin.position.z);

    ////////////////////////////////////////////
    //                                        //
    // from link frame to parent link frame   //
    //                                        //
    ////////////////////////////////////////////

    // un-rotate _link mass into parent link frame
    _link->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(
        phi, theta, psi);
    dRFromEulerAngles(R, -phi,      0,    0);
    dMassRotate(&linkMass, R);
    dRFromEulerAngles(R,    0, -theta,    0);
    dMassRotate(&linkMass, R);
    dRFromEulerAngles(R,    0,      0, -psi);
    dMassRotate(&linkMass, R);

    // un-translate _link mass into parent link frame
    dMassTranslate(&linkMass,
        _link->parent_joint->parent_to_joint_origin_transform.position.x,
        _link->parent_joint->parent_to_joint_origin_transform.position.y,
        _link->parent_joint->parent_to_joint_origin_transform.position.z);

    PrintMass("link in parent link: " + _link->name, linkMass);

    //
    // now linkMass is in the parent frame, add linkMass to parentMass
    // new parentMass should be combined inertia,
    // centered at parent link inertial frame.
    //

    dMassAdd(&parentMass, &linkMass);

    PrintMass("combined: " + _link->getParent()->name, parentMass);

    //
    // Set new combined inertia in parent link frame into parent link urdf
    //

    // save combined mass
    _link->getParent()->inertial->mass = parentMass.mass;

    // save CoG location
    _link->getParent()->inertial->origin.position.x  = parentMass.c[0];
    _link->getParent()->inertial->origin.position.y  = parentMass.c[1];
    _link->getParent()->inertial->origin.position.z  = parentMass.c[2];

    // get MOI at new CoG location
    dMassTranslate(&parentMass,
      -_link->getParent()->inertial->origin.position.x,
      -_link->getParent()->inertial->origin.position.y,
      -_link->getParent()->inertial->origin.position.z);

    // rotate MOI at new CoG location
    _link->getParent()->inertial->origin.rotation.getRPY(phi, theta, psi);
    dRFromEulerAngles(R, phi, theta, psi);
    dMassRotate(&parentMass, R);

    // save new combined MOI
    _link->getParent()->inertial->ixx  = parentMass.I[0+4*0];
    _link->getParent()->inertial->iyy  = parentMass.I[1+4*1];
    _link->getParent()->inertial->izz  = parentMass.I[2+4*2];
    _link->getParent()->inertial->ixy  = parentMass.I[0+4*1];
    _link->getParent()->inertial->ixz  = parentMass.I[0+4*2];
    _link->getParent()->inertial->iyz  = parentMass.I[1+4*2];

    // final urdf inertia check
    PrintMass(_link->getParent());
  }
}

/////////////////////////////////////////////////
/// \brief reduce fixed joints:  lump visuals to parent link
/// \param[in] _link take all visuals from _link and lump/move them
///            to the parent link (_link->getParentLink()).
void ReduceVisualsToParent(UrdfLinkPtr _link)
{
  // lump all visuals of _link to _link->getParent().
  // modify visual name (urdf 0.3.x) or
  //        visual group name (urdf 0.2.x)
  // to indicate that it was lumped (fixed joint reduced)
  // from another descendant link connected by a fixed joint.
  //
  // Algorithm for generating new name (or group name) is:
  //   original name + g_lumpPrefix+original link name (urdf 0.3.x)
  //   original group name + g_lumpPrefix+original link name (urdf 0.2.x)
  // The purpose is to track where this visual came from
  // (original parent link name before lumping/reducing).
#ifndef URDF_GE_0P3
  for (std::map<std::string,
      std::shared_ptr<std::vector<UrdfVisualPtr> > >::iterator
      visualsIt = _link->visual_groups.begin();
      visualsIt != _link->visual_groups.end(); ++visualsIt)
  {
    if (visualsIt->first.find(g_lumpPrefix) == 0)
    {
      // it's a previously lumped mesh, re-lump under same _groupName
      std::string lumpGroupName = visualsIt->first;
      sdfdbg << "re-lumping group name [" << lumpGroupName
             << "] to link [" << _link->getParent()->name << "]\n";
      for (std::vector<UrdfVisualPtr>::iterator
          visualIt = visualsIt->second->begin();
          visualIt != visualsIt->second->end(); ++visualIt)
      {
        // transform visual origin from _link frame to parent link
        // frame before adding to parent
        (*visualIt)->origin = TransformToParentFrame((*visualIt)->origin,
            _link->parent_joint->parent_to_joint_origin_transform);
        // add the modified visual to parent
        ReduceVisualToParent(_link->getParent(), lumpGroupName,
            *visualIt);
      }
    }
    else
    {
      // default and any other groups meshes
      std::string lumpGroupName = g_lumpPrefix+_link->name;
      sdfdbg << "adding modified lump group name [" << lumpGroupName
             << "] to link [" << _link->getParent()->name << "].\n";
      for (std::vector<UrdfVisualPtr>::iterator
          visualIt = visualsIt->second->begin();
          visualIt != visualsIt->second->end(); ++visualIt)
      {
        // transform visual origin from _link frame to
        // parent link frame before adding to parent
        (*visualIt)->origin = TransformToParentFrame((*visualIt)->origin,
            _link->parent_joint->parent_to_joint_origin_transform);
        // add the modified visual to parent
        ReduceVisualToParent(_link->getParent(), lumpGroupName,
            *visualIt);
      }
    }
  }
#else
  for (std::vector<UrdfVisualPtr>::iterator
      visualIt = _link->visual_array.begin();
      visualIt != _link->visual_array.end(); ++visualIt)
  {
    // 20151116: changelog for pull request #235
    std::string newVisualName;
    std::size_t lumpIndex = (*visualIt)->name.find(g_lumpPrefix);
    if (lumpIndex != std::string::npos)
    {
      newVisualName = (*visualIt)->name;
      sdfdbg << "re-lumping visual [" << (*visualIt)->name
             << "] for link [" << _link->name
             << "] to parent [" << _link->getParent()->name
             << "] with name [" << newVisualName << "]\n";
    }
    else
    {
      if ((*visualIt)->name.empty())
      {
        newVisualName = _link->name;
      }
      else
      {
        newVisualName = (*visualIt)->name;
      }
      sdfdbg << "lumping visual [" << (*visualIt)->name
             << "] for link [" << _link->name
             << "] to parent [" << _link->getParent()->name
             << "] with name [" << newVisualName << "]\n";
    }

    // transform visual origin from _link frame to
    // parent link frame before adding to parent
    (*visualIt)->origin = TransformToParentFrame(
        (*visualIt)->origin,
        _link->parent_joint->parent_to_joint_origin_transform);

    // add the modified visual to parent
    ReduceVisualToParent(_link->getParent(), newVisualName,
        *visualIt);
  }
#endif
}

/////////////////////////////////////////////////
/// \brief reduce fixed joints:  lump collisions to parent link
/// \param[in] _link take all collisions from _link and lump/move them
///            to the parent link (_link->getParentLink()).
void ReduceCollisionsToParent(UrdfLinkPtr _link)
{
  // lump all collisions of _link to _link->getParent().
  // modify collision name (urdf 0.3.x) or
  //        collision group name (urdf 0.2.x)
  // to indicate that it was lumped (fixed joint reduced)
  // from another descendant link connected by a fixed joint.
  //
  // Algorithm for generating new name (or group name) is:
  //   original name + g_lumpPrefix+original link name (urdf 0.3.x)
  //   original group name + g_lumpPrefix+original link name (urdf 0.2.x)
  // The purpose is to track where this collision came from
  // (original parent link name before lumping/reducing).
#ifndef URDF_GE_0P3
  for (std::map<std::string,
      std::shared_ptr<std::vector<UrdfCollisionPtr> > >::iterator
      collisionsIt = _link->collision_groups.begin();
      collisionsIt != _link->collision_groups.end(); ++collisionsIt)
  {
    if (collisionsIt->first.find(g_lumpPrefix) == 0)
    {
      // if it's a previously lumped mesh, relump under same _groupName
      std::string lumpGroupName = collisionsIt->first;
      sdfdbg << "re-lumping collision [" << collisionsIt->first
             << "] for link [" << _link->name
             << "] to parent [" << _link->getParent()->name
             << "] with group name [" << lumpGroupName << "]\n";
      for (std::vector<UrdfCollisionPtr>::iterator
          collisionIt = collisionsIt->second->begin();
          collisionIt != collisionsIt->second->end(); ++collisionIt)
      {
        // transform collision origin from _link frame to
        // parent link frame before adding to parent
        (*collisionIt)->origin = TransformToParentFrame(
            (*collisionIt)->origin,
            _link->parent_joint->parent_to_joint_origin_transform);
        // add the modified collision to parent
        ReduceCollisionToParent(_link->getParent(), lumpGroupName,
            *collisionIt);
      }
    }
    else
    {
      // default and any other group meshes
      std::string lumpGroupName = g_lumpPrefix+_link->name;
      sdfdbg << "lumping collision [" << collisionsIt->first
             << "] for link [" << _link->name
             << "] to parent [" << _link->getParent()->name
             << "] with group name [" << lumpGroupName << "]\n";
      for (std::vector<UrdfCollisionPtr>::iterator
          collisionIt = collisionsIt->second->begin();
          collisionIt != collisionsIt->second->end(); ++collisionIt)
      {
        // transform collision origin from _link frame to
        // parent link frame before adding to parent
        (*collisionIt)->origin = TransformToParentFrame(
            (*collisionIt)->origin,
            _link->parent_joint->parent_to_joint_origin_transform);

        // add the modified collision to parent
        ReduceCollisionToParent(_link->getParent(), lumpGroupName,
            *collisionIt);
      }
    }
  }
  // this->PrintCollisionGroups(_link->getParent());
#else
  for (std::vector<UrdfCollisionPtr>::iterator
      collisionIt = _link->collision_array.begin();
      collisionIt != _link->collision_array.end(); ++collisionIt)
  {
    std::string newCollisionName;
    std::size_t lumpIndex =
      (*collisionIt)->name.find(g_lumpPrefix);
    if (lumpIndex != std::string::npos)
    {
      newCollisionName = (*collisionIt)->name;
      sdfdbg << "re-lumping collision [" << (*collisionIt)->name
             << "] for link [" << _link->name
             << "] to parent [" << _link->getParent()->name
             << "] with name [" << newCollisionName << "]\n";
    }
    else
    {
      if ((*collisionIt)->name.empty())
      {
        newCollisionName = _link->name;
      }
      else
      {
        newCollisionName = (*collisionIt)->name;
      }
      sdfdbg << "lumping collision [" << (*collisionIt)->name
             << "] for link [" << _link->name
             << "] to parent [" << _link->getParent()->name
             << "] with name [" << newCollisionName << "]\n";
    }
    // transform collision origin from _link frame to
    // parent link frame before adding to parent
    (*collisionIt)->origin = TransformToParentFrame(
        (*collisionIt)->origin,
        _link->parent_joint->parent_to_joint_origin_transform);

    // add the modified collision to parent
    ReduceCollisionToParent(_link->getParent(), newCollisionName,
        *collisionIt);
  }
#endif
}

/////////////////////////////////////////////////
/// reduce fixed joints:  lump joints to parent link
void ReduceJointsToParent(UrdfLinkPtr _link)
{
  // set child link's parentJoint's parent link to
  // a parent link up stream that does not have a fixed parentJoint
  for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i)
  {
    std::shared_ptr<urdf::Joint> parentJoint =
      _link->child_links[i]->parent_joint;
    if (!FixedJointShouldBeReduced(parentJoint))
    {
      // go down the tree until we hit a parent joint that is not fixed
      UrdfLinkPtr newParentLink = _link;
      ignition::math::Pose3d jointAnchorTransform;
      while (newParentLink->parent_joint &&
          newParentLink->getParent()->name != "world" &&
          FixedJointShouldBeReduced(newParentLink->parent_joint) )
      {
        jointAnchorTransform = jointAnchorTransform * jointAnchorTransform;
        parentJoint->parent_to_joint_origin_transform =
          TransformToParentFrame(
              parentJoint->parent_to_joint_origin_transform,
              newParentLink->parent_joint->parent_to_joint_origin_transform);
        newParentLink = newParentLink->getParent();
      }
      // now set the _link->child_links[i]->parent_joint's parent link to
      // the newParentLink
      _link->child_links[i]->setParent(newParentLink);
      parentJoint->parent_link_name = newParentLink->name;
      // and set the _link->child_links[i]->parent_joint's
      // parent_to_joint_origin_transform as the aggregated anchor transform?
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
std::string lowerStr(const std::string &_str)
{
  std::string out = _str;
  std::transform(out.begin(), out.end(), out.begin(), ::tolower);
  return out;
}

////////////////////////////////////////////////////////////////////////////////
URDF2SDF::URDF2SDF()
{
  // default options
  g_enforceLimits = true;
  g_reduceFixedJoints = true;
}

////////////////////////////////////////////////////////////////////////////////
URDF2SDF::~URDF2SDF()
{
}



////////////////////////////////////////////////////////////////////////////////
std::string Values2str(unsigned int _count, const double *_values)
{
  std::stringstream ss;
  for (unsigned int i = 0 ; i < _count ; ++i)
  {
    if (i > 0)
      ss << " ";
    ss << _values[i];
  }
  return ss.str();
}

////////////////////////////////////////////////////////////////////////////////
void AddKeyValue(TiXmlElement *_elem, const std::string &_key,
    const std::string &_value)
{
  TiXmlElement* childElem = _elem->FirstChildElement(_key);
  if (childElem)
  {
    std::string oldValue = GetKeyValueAsString(childElem);
    if (oldValue != _value)
      sdfwarn << "multiple inconsistent <" << _key
        << "> exists due to fixed joint reduction"
        << " overwriting previous value [" << oldValue
        << "] with [" << _value << "].\n";
    else
       sdfdbg << "multiple consistent <" << _key
              << "> exists with [" << _value
              << "] due to fixed joint reduction.\n";
    _elem->RemoveChild(childElem);  // remove old _elem
  }

  TiXmlElement *ekey = new TiXmlElement(_key);
  TiXmlText *textEkey = new TiXmlText(_value);
  ekey->LinkEndChild(textEkey);
  _elem->LinkEndChild(ekey);
}

////////////////////////////////////////////////////////////////////////////////
void AddTransform(TiXmlElement *_elem, const ignition::math::Pose3d &_transform)
{
  ignition::math::Vector3d e = _transform.Rot().Euler();
  double cpose[6] = { _transform.Pos().X(), _transform.Pos().Y(),
    _transform.Pos().Z(), e.X(), e.Y(), e.Z() };

  // set geometry transform
  AddKeyValue(_elem, "pose", Values2str(6, cpose));
}

////////////////////////////////////////////////////////////////////////////////
std::string GetKeyValueAsString(TiXmlElement* _elem)
{
  std::string valueStr;
  if (_elem->Attribute("value"))
  {
    valueStr = _elem->Attribute("value");
  }
  else if (_elem->FirstChild())
    /// @todo: FIXME: comment out check for now, different tinyxml
    /// versions fails to compile:
    //  && _elem->FirstChild()->Type() == TiXmlNode::TINYXML_TEXT)
  {
    valueStr = _elem->FirstChild()->ValueStr();
  }
  return valueStr;
}

/////////////////////////////////////////////////
void ParseRobotOrigin(TiXmlDocument &_urdfXml)
{
  TiXmlElement *robotXml = _urdfXml.FirstChildElement("robot");
  TiXmlElement *originXml = robotXml->FirstChildElement("origin");
  if (originXml)
  {
    g_initialRobotPose.position = ParseVector3(
        std::string(originXml->Attribute("xyz")));
    urdf::Vector3 rpy = ParseVector3(std::string(originXml->Attribute("rpy")));
    g_initialRobotPose.rotation.setFromRPY(rpy.x, rpy.y, rpy.z);
    g_initialRobotPoseValid = true;
  }
}

/////////////////////////////////////////////////
void InsertRobotOrigin(TiXmlElement *_elem)
{
  if (g_initialRobotPoseValid)
  {
    // set transform
    double pose[6];
    pose[0] = g_initialRobotPose.position.x;
    pose[1] = g_initialRobotPose.position.y;
    pose[2] = g_initialRobotPose.position.z;
    g_initialRobotPose.rotation.getRPY(pose[3], pose[4], pose[5]);
    AddKeyValue(_elem, "pose", Values2str(6, pose));
  }
}

////////////////////////////////////////////////////////////////////////////////
void URDF2SDF::ParseSDFExtension(TiXmlDocument &_urdfXml)
{
  TiXmlElement* robotXml = _urdfXml.FirstChildElement("robot");

  // Get all SDF extension elements, put everything in
  //   g_extensions map, containing a key string
  //   (link/joint name) and values
  for (TiXmlElement* sdfXml = robotXml->FirstChildElement("gazebo");
      sdfXml; sdfXml = sdfXml->NextSiblingElement("gazebo"))
  {
    const char* ref = sdfXml->Attribute("reference");
    std::string refStr;
    if (!ref)
    {
      // copy extensions for robot (outside of link/joint)
      refStr.clear();
    }
    else
    {
      // copy extensions for link/joint
      refStr = std::string(ref);
    }

    if (g_extensions.find(refStr) ==
        g_extensions.end())
    {
      // create extension map for reference
      std::vector<SDFExtensionPtr> ge;
      g_extensions.insert(std::make_pair(refStr, ge));
    }

    // create and insert a new SDFExtension into the map
    SDFExtensionPtr sdf(new SDFExtension());

    // begin parsing xml node
    for (TiXmlElement *childElem = sdfXml->FirstChildElement();
        childElem; childElem = childElem->NextSiblingElement())
    {
      sdf->oldLinkName = refStr;

      // go through all elements of the extension,
      //   extract what we know, and save the rest in blobs
      // @todo:  somehow use sdf definitions here instead of hard coded
      //         objects

      // material
      if (childElem->ValueStr() == "material")
      {
        sdf->material = GetKeyValueAsString(childElem);
      }
      else if (childElem->ValueStr() == "collision"
            || childElem->ValueStr() == "visual")
      {
        // anything inside of collision or visual tags:
        // <gazebo reference="link_name">
        //   <collision>
        //     <collision_extention_stuff_here/>
        //   </collision>
        //   <visual>
        //     <visual_extention_stuff_here/>
        //   </visual>
        // </gazebl>
        // are treated as blobs that gets inserted
        // into all collisions and visuals for the link
        // <collision name="link_name[anything here]">
        //   <stuff_from_urdf_link_collisions/>
        //   <collision_extention_stuff_here/>
        // </collision>
        // <visual name="link_name[anything here]">
        //   <stuff_from_urdf_link_visuals/>
        //   <visual_extention_stuff_here/>
        // </visual>

        // a place to store converted doc
        for (TiXmlElement* e = childElem->FirstChildElement(); e;
            e = e->NextSiblingElement())
        {
          TiXmlDocument xmlNewDoc;

          std::ostringstream origStream;
          origStream << *e;
          xmlNewDoc.Parse(origStream.str().c_str());

          // save all unknown stuff in a vector of blobs
          TiXmlElementPtr blob(
            new TiXmlElement(*xmlNewDoc.FirstChildElement()));
          if (childElem->ValueStr() == "collision")
          {
            sdf->collision_blobs.push_back(blob);
          }
          else
          {
            sdf->visual_blobs.push_back(blob);
          }
        }
      }
      else if (childElem->ValueStr() == "static")
      {
        std::string valueStr = GetKeyValueAsString(childElem);

        // default of setting static flag is false
        if (lowerStr(valueStr) == "true" || lowerStr(valueStr) == "yes" ||
            valueStr == "1")
          sdf->setStaticFlag = true;
        else
          sdf->setStaticFlag = false;
      }
      else if (childElem->ValueStr() == "turnGravityOff")
      {
        std::string valueStr = GetKeyValueAsString(childElem);

        // default of gravity is true
        if (lowerStr(valueStr) == "false" || lowerStr(valueStr) == "no" ||
            valueStr == "0")
          sdf->gravity = true;
        else
          sdf->gravity = false;
      }
      else if (childElem->ValueStr() == "dampingFactor")
      {
        sdf->isDampingFactor = true;
        sdf->dampingFactor = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "maxVel")
      {
        sdf->isMaxVel = true;
        sdf->maxVel = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "minDepth")
      {
        sdf->isMinDepth = true;
        sdf->minDepth = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "mu1")
      {
        sdf->isMu1 = true;
        sdf->mu1 = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "mu2")
      {
        sdf->isMu2 = true;
        sdf->mu2 = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "fdir1")
      {
        sdf->fdir1 = GetKeyValueAsString(childElem);
      }
      else if (childElem->ValueStr() == "kp")
      {
        sdf->isKp = true;
        sdf->kp = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "kd")
      {
        sdf->isKd = true;
        sdf->kd = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "selfCollide")
      {
        sdf->isSelfCollide = true;
        std::string valueStr = GetKeyValueAsString(childElem);

        // default of selfCollide is false
        if (lowerStr(valueStr) == "true" || lowerStr(valueStr) == "yes" ||
            valueStr == "1")
          sdf->selfCollide = true;
        else
          sdf->selfCollide = false;
      }
      else if (childElem->ValueStr() == "maxContacts")
      {
        sdf->isMaxContacts = true;
        sdf->maxContacts = boost::lexical_cast<int>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "laserRetro")
      {
        sdf->isLaserRetro = true;
        sdf->laserRetro = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "springReference")
      {
        sdf->isSpringReference = true;
        sdf->springReference = std::stod(GetKeyValueAsString(childElem));
      }
      else if (childElem->ValueStr() == "springStiffness")
      {
        sdf->isSpringStiffness = true;
        sdf->springStiffness = std::stod(GetKeyValueAsString(childElem));
      }
      else if (childElem->ValueStr() == "stopCfm")
      {
        sdf->isStopCfm = true;
        sdf->stopCfm = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "stopErp")
      {
        sdf->isStopErp = true;
        sdf->stopErp = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "stopKp")
      {
        sdf->isStopKp = true;
        sdf->stopKp = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "stopKd")
      {
        sdf->isStopKd = true;
        sdf->stopKd = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "initialJointPosition")
      {
        sdf->isInitialJointPosition = true;
        sdf->initialJointPosition = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "fudgeFactor")
      {
        sdf->isFudgeFactor = true;
        sdf->fudgeFactor = boost::lexical_cast<double>(
            GetKeyValueAsString(childElem).c_str());
      }
      else if (childElem->ValueStr() == "provideFeedback")
      {
        sdf->isProvideFeedback = true;
        std::string valueStr = GetKeyValueAsString(childElem);

        if (lowerStr(valueStr) == "true" || lowerStr(valueStr) == "yes" ||
            valueStr == "1")
          sdf->provideFeedback = true;
        else
          sdf->provideFeedback = false;
      }
      else if (childElem->ValueStr() == "canonicalBody")
      {
        sdfdbg << "do nothing with canonicalBody\n";
      }
      else if (childElem->ValueStr() == "cfmDamping" ||
               childElem->ValueStr() == "implicitSpringDamper")
      {
        if (childElem->ValueStr() == "cfmDamping")
          sdfwarn << "Note that cfmDamping is being deprecated by "
                  << "implicitSpringDamper, please replace instances "
                  << "of cfmDamping with implicitSpringDamper in your model.\n";

        sdf->isImplicitSpringDamper = true;
        std::string valueStr = GetKeyValueAsString(childElem);

        if (lowerStr(valueStr) == "true" || lowerStr(valueStr) == "yes" ||
            valueStr == "1")
          sdf->implicitSpringDamper = true;
        else
          sdf->implicitSpringDamper = false;
      }
      else if (childElem->ValueStr() == "disableFixedJointLumping")
      {
        std::string valueStr = GetKeyValueAsString(childElem);

        if (lowerStr(valueStr) == "true" || lowerStr(valueStr) == "yes" ||
            valueStr == "1")
        {
          g_fixedJointsNotReduced.insert(refStr);
        }
      }
      else
      {
        // a place to store converted doc
        TiXmlDocument xmlNewDoc;

        std::ostringstream stream;
        stream << *childElem;
        sdfdbg << "extension [" << stream.str() <<
          "] not converted from URDF, probably already in SDF format.\n";
        xmlNewDoc.Parse(stream.str().c_str());

        // save all unknown stuff in a vector of blobs
        TiXmlElementPtr blob(new TiXmlElement(*xmlNewDoc.FirstChildElement()));
        sdf->blobs.push_back(blob);
      }
    }

    // insert into my map
    (g_extensions.find(refStr))->second.push_back(sdf);
  }
}

////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionCollision(TiXmlElement *_elem,
    const std::string &_linkName)
{
  // loop through extensions for the whole model
  // and see which ones belong to _linkName
  // This might be complicated since there's:
  //   - urdf collision name -> sdf collision name conversion
  //   - fixed joint reduction / lumping
  for (StringSDFExtensionPtrMap::iterator
      sdfIt = g_extensions.begin();
      sdfIt != g_extensions.end(); ++sdfIt)
  {
    if (sdfIt->first == _linkName)
    {
      // std::cerr << "============================\n";
      // std::cerr << "working on g_extensions for link ["
      //           << sdfIt->first << "]\n";
      // if _elem already has a surface element, use it
      TiXmlNode *surface = _elem->FirstChild("surface");
      TiXmlNode *friction = NULL;
      TiXmlNode *frictionOde = NULL;
      TiXmlNode *contact = NULL;
      TiXmlNode *contactOde = NULL;

      // loop through all the gazebo extensions stored in sdfIt->second
      for (std::vector<SDFExtensionPtr>::iterator ge = sdfIt->second.begin();
          ge != sdfIt->second.end(); ++ge)
      {
        // Check if this blob belongs to _elem based on
        //   - blob's reference link name (_linkName or sdfIt->first)
        //   - _elem (destination for blob, which is a collision sdf).

        if (!_elem->Attribute("name"))
        {
          sdferr << "ERROR: collision _elem has no name,"
                 << " something is wrong" << "\n";
        }

        std::string sdfCollisionName(_elem->Attribute("name"));

        // std::cerr << "----------------------------\n";
        // std::cerr << "blob belongs to [" << _linkName
        //           << "] with old parent LinkName [" << (*ge)->oldLinkName
        //           << "]\n";
        // std::cerr << "_elem sdf collision name [" << sdfCollisionName
        //           << "]\n";
        // std::cerr << "----------------------------\n";

        std::string lumpCollisionName = g_lumpPrefix +
          (*ge)->oldLinkName + g_collisionExt;

        bool wasReduced = (_linkName == (*ge)->oldLinkName);
        bool collisionNameContainsLinkname =
          sdfCollisionName.find(_linkName) != std::string::npos;
        bool collisionNameContainsLumpedLinkname =
          sdfCollisionName.find(lumpCollisionName) != std::string::npos;
        bool collisionNameContainsLumpedRef =
          sdfCollisionName.find(g_lumpPrefix) != std::string::npos;

        if (!collisionNameContainsLinkname)
        {
          sdferr << "collision name does not contain link name,"
                 << " file an issue.\n";
        }

        // if the collision _elem was not reduced,
        // its name should not have g_lumpPrefix in it.
        // otherwise, its name should have
        // "g_lumpPrefix+[original link name before reduction]".
        if ((wasReduced && !collisionNameContainsLumpedRef) ||
            (!wasReduced && collisionNameContainsLumpedLinkname))
        {
          // insert any blobs (including visual plugins)
          // warning, if you insert a <surface> sdf here, it might
          // duplicate what was constructed above.
          // in the future, we should use blobs (below) in place of
          // explicitly specified fields (above).
          if (!(*ge)->collision_blobs.empty())
          {
            std::vector<TiXmlElementPtr>::iterator blob;
            for (blob = (*ge)->collision_blobs.begin();
                blob != (*ge)->collision_blobs.end(); ++blob)
            {
              // find elements and assign pointers if they exist
              // for mu1, mu2, minDepth, maxVel, fdir1, kp, kd
              // otherwise, they are allocated by 'new' below.
              // std::cerr << ">>>>> working on extension blob: ["
              //           << (*blob)->Value() << "]\n";

              // print for debug
              std::ostringstream origStream;
              std::unique_ptr<TiXmlNode> blobClone((*blob)->Clone());
              origStream << *blobClone;
              // std::cerr << "collision extension ["
              //           << origStream.str() << "]\n";

              if (strcmp((*blob)->Value(), "surface") == 0)
              {
                // blob is a <surface>, tread carefully otherwise
                // we end up with multiple copies of <surface>.
                // Also, get pointers (contact[Ode], friction[Ode])
                // below for backwards (non-blob) compatibility.
                if (surface == NULL)
                {
                  // <surface> do not exist, it simple,
                  // just add it to the current collision
                  // and it's done.
                  _elem->LinkEndChild((*blob)->Clone());
                  surface = _elem->LastChild("surface");
                  // std::cerr << " --- surface created "
                  //           <<  (void*)surface << "\n";
                }
                else
                {
                  // <surface> exist already, remove it and
                  // overwrite with the blob.
                  _elem->RemoveChild(surface);
                  _elem->LinkEndChild((*blob)->Clone());
                  surface = _elem->FirstChild("surface");
                  // std::cerr << " --- surface exists, replace with blob.\n";
                }

                // Extra code for backwards compatibility, to
                // deal with old way of specifying collision attributes
                // using individual elements listed below:
                //   "mu"
                //   "mu2"
                //   "fdir1"
                //   "kp"
                //   "kd"
                //   "max_vel"
                //   "min_depth"
                //   "laser_retro"
                //   "max_contacts"
                // Get contact[Ode] and friction[Ode] node pointers
                // if they exist.
                contact  = surface->FirstChild("contact");
                if (contact != NULL)
                {
                  contactOde  = contact->FirstChild("ode");
                }
                friction = surface->FirstChild("friction");
                if (friction != NULL)
                {
                  frictionOde  = friction->FirstChild("ode");
                }
              }
              else
              {
                // If the blob is not a <surface>, we don't have
                // to worry about backwards compatibility.
                // Simply add to master element.
                _elem->LinkEndChild((*blob)->Clone());
              }
            }
          }

          // Extra code for backwards compatibility, to
          // deal with old way of specifying collision attributes
          // using individual elements listed below:
          //   "mu"
          //   "mu2"
          //   "fdir1"
          //   "kp"
          //   "kd"
          //   "max_vel"
          //   "min_depth"
          //   "laser_retro"
          //   "max_contacts"
          // The new way to do this is to specify everything
          // in collision blobs by using the <collision> tag.
          // So there's no need for custom code for each property.

          // construct new elements if not in blobs
          if (surface == NULL)
          {
            surface  = new TiXmlElement("surface");
            if (!surface)
            {
              // Memory allocation error
              sdferr << "Memory allocation error while"
                     << " processing <surface>.\n";
            }
            _elem->LinkEndChild(surface);
          }

          // construct new elements if not in blobs
          if (contact == NULL)
          {
            if (surface->FirstChild("contact") == NULL)
            {
              contact  = new TiXmlElement("contact");
              if (!contact)
              {
                // Memory allocation error
                sdferr << "Memory allocation error while"
                       << " processing <contact>.\n";
              }
              surface->LinkEndChild(contact);
            }
            else
            {
              contact  = surface->FirstChild("contact");
            }
          }

          if (contactOde == NULL)
          {

            if (contact->FirstChild("ode") == NULL)
            {
              contactOde  = new TiXmlElement("ode");
              if (!contactOde)
              {
                // Memory allocation error
                sdferr << "Memory allocation error while"
                       << " processing <contact><ode>.\n";
              }
              contact->LinkEndChild(contactOde);
            }
            else
            {
              contactOde  = contact->FirstChild("ode");
            }
          }

          if (friction == NULL)
          {
            if (surface->FirstChild("friction") == NULL)
            {
              friction  = new TiXmlElement("friction");
              if (!friction)
              {
                // Memory allocation error
                sdferr << "Memory allocation error while"
                       << " processing <friction>.\n";
              }
              surface->LinkEndChild(friction);
            }
            else
            {
              friction  = surface->FirstChild("friction");
            }
          }

          if (frictionOde == NULL)
          {
            if (friction->FirstChild("ode") == NULL)
            {
              frictionOde  = new TiXmlElement("ode");
              if (!frictionOde)
              {
                // Memory allocation error
                sdferr << "Memory allocation error while"
                       << " processing <friction><ode>.\n";
              }
              friction->LinkEndChild(frictionOde);
            }
            else
            {
              frictionOde  = friction->FirstChild("ode");
            }
          }

          // insert mu1, mu2, kp, kd for collision
          if ((*ge)->isMu1)
          {
            AddKeyValue(frictionOde->ToElement(), "mu",
                Values2str(1, &(*ge)->mu1));
          }
          if ((*ge)->isMu2)
          {
            AddKeyValue(frictionOde->ToElement(), "mu2",
                Values2str(1, &(*ge)->mu2));
          }
          if (!(*ge)->fdir1.empty())
          {
            AddKeyValue(frictionOde->ToElement(), "fdir1", (*ge)->fdir1);
          }
          if ((*ge)->isKp)
          {
            AddKeyValue(contactOde->ToElement(), "kp",
                        Values2str(1, &(*ge)->kp));
          }
          if ((*ge)->isKd)
          {
            AddKeyValue(contactOde->ToElement(), "kd",
                        Values2str(1, &(*ge)->kd));
          }
          // max contact interpenetration correction velocity
          if ((*ge)->isMaxVel)
          {
            AddKeyValue(contactOde->ToElement(), "max_vel",
                Values2str(1, &(*ge)->maxVel));
          }
          // contact interpenetration margin tolerance
          if ((*ge)->isMinDepth)
          {
            AddKeyValue(contactOde->ToElement(), "min_depth",
                Values2str(1, &(*ge)->minDepth));
          }
          if ((*ge)->isLaserRetro)
          {
            AddKeyValue(_elem, "laser_retro",
                Values2str(1, &(*ge)->laserRetro));
          }
          if ((*ge)->isMaxContacts)
          {
            AddKeyValue(_elem, "max_contacts",
                boost::lexical_cast<std::string>((*ge)->maxContacts));
          }
        }
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionVisual(TiXmlElement *_elem,
    const std::string &_linkName)
{
  // loop through extensions for the whole model
  // and see which ones belong to _linkName
  // This might be complicated since there's:
  //   - urdf visual name -> sdf visual name conversion
  //   - fixed joint reduction / lumping
  for (StringSDFExtensionPtrMap::iterator
      sdfIt = g_extensions.begin();
      sdfIt != g_extensions.end(); ++sdfIt)
  {
    if (sdfIt->first == _linkName)
    {
      // std::cerr << "============================\n";
      // std::cerr << "working on g_extensions for link ["
      //           << sdfIt->first << "]\n";
      // if _elem already has a material element, use it
      TiXmlNode *material = _elem->FirstChild("material");
      TiXmlElement *script = NULL;

      // loop through all the gazebo extensions stored in sdfIt->second
      for (std::vector<SDFExtensionPtr>::iterator ge = sdfIt->second.begin();
          ge != sdfIt->second.end(); ++ge)
      {
        // Check if this blob belongs to _elem based on
        //   - blob's reference link name (_linkName or sdfIt->first)
        //   - _elem (destination for blob, which is a visual sdf).

        if (!_elem->Attribute("name"))
          sdferr << "ERROR: visual _elem has no name,"
                 << " something is wrong" << "\n";

        std::string sdfVisualName(_elem->Attribute("name"));

        // std::cerr << "----------------------------\n";
        // std::cerr << "blob belongs to [" << _linkName
        //           << "] with old parent LinkName [" << (*ge)->oldLinkName
        //           << "]\n";
        // std::cerr << "_elem sdf visual name [" << sdfVisualName
        //           << "]\n";
        // std::cerr << "----------------------------\n";

        std::string lumpVisualName = g_lumpPrefix +
          (*ge)->oldLinkName + g_visualExt;

        bool wasReduced = (_linkName == (*ge)->oldLinkName);
        bool visualNameContainsLinkname =
          sdfVisualName.find(_linkName) != std::string::npos;
        bool visualNameContainsLumpedLinkname =
          sdfVisualName.find(lumpVisualName) != std::string::npos;
        bool visualNameContainsLumpedRef =
          sdfVisualName.find(g_lumpPrefix) != std::string::npos;

        if (!visualNameContainsLinkname)
          sdferr << "visual name does not contain link name,"
                 << " file an issue.\n";

        // if the visual _elem was not reduced,
        // its name should not have g_lumpPrefix in it.
        // otherwise, its name should have
        // "g_lumpPrefix+[original link name before reduction]".
        if ((wasReduced && !visualNameContainsLumpedRef) ||
            (!wasReduced && visualNameContainsLumpedLinkname))
        {
          // insert any blobs (including visual plugins)
          // warning, if you insert a <material> sdf here, it might
          // duplicate what was constructed above.
          // in the future, we should use blobs (below) in place of
          // explicitly specified fields (above).
          if (!(*ge)->visual_blobs.empty())
          {
            std::vector<TiXmlElementPtr>::iterator blob;
            for (blob = (*ge)->visual_blobs.begin();
                blob != (*ge)->visual_blobs.end(); ++blob)
            {
              // find elements and assign pointers if they exist
              // for mu1, mu2, minDepth, maxVel, fdir1, kp, kd
              // otherwise, they are allocated by 'new' below.
              // std::cerr << ">>>>> working on extension blob: ["
              //           << (*blob)->Value() << "]\n";

              // print for debug
              // std::ostringstream origStream;
              // origStream << *(*blob)->Clone();
              // std::cerr << "visual extension ["
              //           << origStream.str() << "]\n";

              if (strcmp((*blob)->Value(), "material") == 0)
              {
                // blob is a <material>, tread carefully otherwise
                // we end up with multiple copies of <material>.
                // Also, get pointers (script)
                // below for backwards (non-blob) compatibility.
                if (material == NULL)
                {
                  // <material> do not exist, it simple,
                  // just add it to the current visual
                  // and it's done.
                  _elem->LinkEndChild((*blob)->Clone());
                  material = _elem->LastChild("material");
                  // std::cerr << " --- material created "
                  //           <<  (void*)material << "\n";
                }
                else
                {
                  // <material> exist already, remove it and
                  // overwrite with the blob.
                  _elem->RemoveChild(material);
                  _elem->LinkEndChild((*blob)->Clone());
                  material = _elem->FirstChild("material");
                  // std::cerr << " --- material exists, replace with blob.\n";
                }

                // Extra code for backwards compatibility, to
                // deal with old way of specifying visual attributes
                // using individual element:
                //   "script"
                // Get script node pointers
                // if they exist.
                script = material->FirstChildElement("script");
              }
              else
              {
                // std::cerr << "***** working on extension blob: ["
                //           << (*blob)->Value() << "]\n";
                // If the blob is not a <material>, we don't have
                // to worry about backwards compatibility.
                // Simply add to master element.
                _elem->LinkEndChild((*blob)->Clone());
              }
            }
          }

          // Extra code for backwards compatibility, to
          // deal with old way of specifying visual attributes
          // using individual element:
          //   "script"
          // The new way to do this is to specify everything
          // in visual blobs by using the <visual> tag.
          // So there's no need for custom code for each property.

          // construct new elements if not in blobs
          if (material == NULL)
          {
            material  = new TiXmlElement("material");
            if (!material)
            {
              // Memory allocation error
              sdferr << "Memory allocation error while"
                     << " processing <material>.\n";
            }
            _elem->LinkEndChild(material);
          }

          if (script == NULL)
          {
            if (material->FirstChildElement("script") == NULL)
            {
              script  = new TiXmlElement("script");
              if (!script)
              {
                // Memory allocation error
                sdferr << "Memory allocation error while"
                       << " processing <script>.\n";
              }
              material->LinkEndChild(script);
            }
            else
            {
              script  = material->FirstChildElement("script");
            }
          }

          // backward compatibility for old code
          // insert material/script block for visual
          // (*ge)->material block goes under sdf <material><script><name>.
          if (!(*ge)->material.empty())
          {
            AddKeyValue(script, "name", (*ge)->material);
            // hard code original default gazebo materials files
            AddKeyValue(script, "uri",
              "file://media/materials/scripts/gazebo.material");
          }
        }
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName)
{
  for (StringSDFExtensionPtrMap::iterator
       sdfIt = g_extensions.begin();
       sdfIt != g_extensions.end(); ++sdfIt)
  {
    if (sdfIt->first == _linkName)
    {
      sdfdbg << "inserting extension with reference ["
             << _linkName << "] into link.\n";
      for (std::vector<SDFExtensionPtr>::iterator ge =
          sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge)
      {
        // insert gravity
        if ((*ge)->gravity)
          AddKeyValue(_elem, "gravity", "true");
        else
          AddKeyValue(_elem, "gravity", "false");

        // damping factor
        TiXmlElement *velocityDecay = new TiXmlElement("velocity_decay");
        if ((*ge)->isDampingFactor)
        {
          /// @todo separate linear and angular velocity decay
          AddKeyValue(velocityDecay, "linear",
              Values2str(1, &(*ge)->dampingFactor));
          AddKeyValue(velocityDecay, "angular",
              Values2str(1, &(*ge)->dampingFactor));
        }
        _elem->LinkEndChild(velocityDecay);
        // selfCollide tag
        if ((*ge)->isSelfCollide)
        {
          AddKeyValue(_elem, "self_collide", (*ge)->selfCollide ? "1" : "0");
        }
        // insert blobs into body
        for (std::vector<TiXmlElementPtr>::iterator
            blobIt = (*ge)->blobs.begin();
            blobIt != (*ge)->blobs.end(); ++blobIt)
        {
          _elem->LinkEndChild((*blobIt)->Clone());
        }

      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionJoint(TiXmlElement *_elem,
    const std::string &_jointName)
{
  for (StringSDFExtensionPtrMap::iterator
      sdfIt = g_extensions.begin();
      sdfIt != g_extensions.end(); ++sdfIt)
  {
    if (sdfIt->first == _jointName)
    {
      for (std::vector<SDFExtensionPtr>::iterator
          ge = sdfIt->second.begin();
          ge != sdfIt->second.end(); ++ge)
      {

        TiXmlElement *physics = _elem->FirstChildElement("physics");
        bool newPhysics = false;
        if (physics == NULL)
        {
          physics = new TiXmlElement("physics");
          newPhysics = true;
        }

        TiXmlElement *physicsOde = physics->FirstChildElement("ode");
        bool newPhysicsOde = false;
        if (physicsOde == NULL)
        {
          physicsOde = new TiXmlElement("ode");
          newPhysicsOde = true;
        }

        TiXmlElement *limit = physicsOde->FirstChildElement("limit");
        bool newLimit = false;
        if (limit == NULL)
        {
          limit = new TiXmlElement("limit");
          newLimit = true;
        }

        TiXmlElement *axis = _elem->FirstChildElement("axis");
        bool newAxis = false;
        if (axis == NULL)
        {
          axis = new TiXmlElement("axis");
          newAxis = true;
        }

        TiXmlElement *dynamics = axis->FirstChildElement("dynamics");
        bool newDynamics = false;
        if (dynamics == NULL)
        {
          dynamics = new TiXmlElement("dynamics");
          newDynamics = true;
        }

        // insert stopCfm, stopErp, fudgeFactor
        if ((*ge)->isStopCfm)
        {
          AddKeyValue(limit, "cfm", Values2str(1, &(*ge)->stopCfm));
        }
        if ((*ge)->isStopErp)
        {
          AddKeyValue(limit, "erp", Values2str(1, &(*ge)->stopErp));
        }
        if ((*ge)->isSpringReference)
        {
          AddKeyValue(dynamics, "spring_reference",
            Values2str(1, &(*ge)->springReference));
        }
        if ((*ge)->isSpringStiffness)
        {
          AddKeyValue(dynamics, "spring_stiffness",
            Values2str(1, &(*ge)->springStiffness));
        }
        // if ((*ge)->isInitialJointPosition)
        //    AddKeyValue(_elem, "initialJointPosition",
        //      Values2str(1, &(*ge)->initialJointPosition));

        // insert provideFeedback
        if ((*ge)->isProvideFeedback)
        {
          if ((*ge)->provideFeedback)
          {
            AddKeyValue(physics, "provide_feedback", "true");
            AddKeyValue(physicsOde, "provide_feedback", "true");
          }
          else
          {
            AddKeyValue(physics, "provide_feedback", "false");
            AddKeyValue(physicsOde, "provide_feedback", "false");
          }
        }

        // insert implicitSpringDamper
        if ((*ge)->isImplicitSpringDamper)
        {
          if ((*ge)->implicitSpringDamper)
          {
            AddKeyValue(physicsOde, "implicit_spring_damper", "true");
            /// \TODO: deprecating cfm_damping, transitional tag below
            AddKeyValue(physicsOde, "cfm_damping", "true");
          }
          else
          {
            AddKeyValue(physicsOde, "implicit_spring_damper", "false");
            /// \TODO: deprecating cfm_damping, transitional tag below
            AddKeyValue(physicsOde, "cfm_damping", "false");
          }
        }

        // insert fudgeFactor
        if ((*ge)->isFudgeFactor)
          AddKeyValue(physicsOde, "fudge_factor",
              Values2str(1, &(*ge)->fudgeFactor));

        if (newDynamics)
          axis->LinkEndChild(dynamics);
        if (newAxis)
          _elem->LinkEndChild(axis);

        if (newLimit)
          physicsOde->LinkEndChild(limit);
        if (newPhysicsOde)
          physics->LinkEndChild(physicsOde);
        if (newPhysics)
          _elem->LinkEndChild(physics);

        // insert all additional blobs into joint
        for (std::vector<TiXmlElementPtr>::iterator
            blobIt = (*ge)->blobs.begin();
            blobIt != (*ge)->blobs.end(); ++blobIt)
        {
          _elem->LinkEndChild((*blobIt)->Clone());
        }
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionRobot(TiXmlElement *_elem)
{
  for (StringSDFExtensionPtrMap::iterator
      sdfIt = g_extensions.begin();
      sdfIt != g_extensions.end(); ++sdfIt)
  {
    if (sdfIt->first.empty())
    {
      // no reference specified
      for (std::vector<SDFExtensionPtr>::iterator
          ge = sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge)
      {
        // insert static flag
        if ((*ge)->setStaticFlag)
          AddKeyValue(_elem, "static", "true");
        else
          AddKeyValue(_elem, "static", "false");

        // copy extension containing blobs and without reference
        for (std::vector<TiXmlElementPtr>::iterator
            blobIt = (*ge)->blobs.begin();
            blobIt != (*ge)->blobs.end(); ++blobIt)
        {
          std::ostringstream streamIn;
          streamIn << *(*blobIt);
          _elem->LinkEndChild((*blobIt)->Clone());
        }
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void CreateGeometry(TiXmlElement* _elem,
    std::shared_ptr<urdf::Geometry> _geom)
{
  TiXmlElement *sdfGeometry = new TiXmlElement("geometry");

  std::string type;
  TiXmlElement *geometryType = NULL;

  switch (_geom->type)
  {
    case urdf::Geometry::BOX:
      type = "box";
      {
        std::shared_ptr<const urdf::Box> box;
        box = std::dynamic_pointer_cast< const urdf::Box >(_geom);
        int sizeCount = 3;
        double sizeVals[3];
        sizeVals[0] = box->dim.x;
        sizeVals[1] = box->dim.y;
        sizeVals[2] = box->dim.z;
        geometryType = new TiXmlElement(type);
        AddKeyValue(geometryType, "size",
            Values2str(sizeCount, sizeVals));
      }
      break;
    case urdf::Geometry::CYLINDER:
      type = "cylinder";
      {
        std::shared_ptr<const urdf::Cylinder> cylinder;
        cylinder = std::dynamic_pointer_cast<const urdf::Cylinder >(_geom);
        geometryType = new TiXmlElement(type);
        AddKeyValue(geometryType, "length",
            Values2str(1, &cylinder->length));
        AddKeyValue(geometryType, "radius",
            Values2str(1, &cylinder->radius));
      }
      break;
    case urdf::Geometry::SPHERE:
      type = "sphere";
      {
        std::shared_ptr<const urdf::Sphere> sphere;
        sphere = std::dynamic_pointer_cast<const urdf::Sphere >(_geom);
        geometryType = new TiXmlElement(type);
        AddKeyValue(geometryType, "radius",
            Values2str(1, &sphere->radius));
      }
      break;
    case urdf::Geometry::MESH:
      type = "mesh";
      {
        std::shared_ptr<const urdf::Mesh> mesh;
        mesh = std::dynamic_pointer_cast<const urdf::Mesh >(_geom);
        geometryType = new TiXmlElement(type);
        AddKeyValue(geometryType, "scale", Vector32Str(mesh->scale));
        // do something more to meshes
        {
          // set mesh file
          if (mesh->filename.empty())
          {
            sdferr << "urdf2sdf: mesh geometry with no filename given.\n";
          }

          // give some warning if file does not exist.
          // disabled while switching to uri
          // @todo: re-enable check
          // std::ifstream fin;
          // fin.open(mesh->filename.c_str(), std::ios::in);
          // fin.close();
          // if (fin.fail())
          //   sdfwarn << "filename referred by mesh ["
          //          << mesh->filename << "] does not appear to exist.\n";

          // Convert package:// to model://,
          // in ROS, this will work if
          // the model package is in ROS_PACKAGE_PATH and has a manifest.xml
          // as a typical ros package does.
          std::string modelFilename = mesh->filename;
          std::string packagePrefix("package://");
          std::string modelPrefix("model://");
          size_t pos1 = modelFilename.find(packagePrefix, 0);
          if (pos1 != std::string::npos)
          {
            size_t repLen = packagePrefix.size();
            modelFilename.replace(pos1, repLen, modelPrefix);
            // sdfwarn << "ros style uri [package://] is"
            //   << "automatically converted: [" << modelFilename
            //   << "], make sure your ros package is in GAZEBO_MODEL_PATH"
            //   << " and switch your manifest to conform to sdf's"
            //   << " model database format.  See ["
            //   << "http://sdfsim.org/wiki/Model_database#Model_Manifest_XML"
            //   << "] for more info.\n";
          }

          // add mesh filename
          AddKeyValue(geometryType, "uri", modelFilename);
        }
      }
      break;
    default:
      sdfwarn << "Unknown body type: [" << static_cast<int>(_geom->type)
        << "] skipped in geometry\n";
      break;
  }

  if (geometryType)
  {
    sdfGeometry->LinkEndChild(geometryType);
    _elem->LinkEndChild(sdfGeometry);
  }
}

////////////////////////////////////////////////////////////////////////////////
std::string GetGeometryBoundingBox(
    std::shared_ptr<urdf::Geometry> _geom, double *_sizeVals)
{
  std::string type;

  switch (_geom->type)
  {
    case urdf::Geometry::BOX:
      type = "box";
      {
        std::shared_ptr<const urdf::Box> box;
        box = std::dynamic_pointer_cast<const urdf::Box >(_geom);
        _sizeVals[0] = box->dim.x;
        _sizeVals[1] = box->dim.y;
        _sizeVals[2] = box->dim.z;
      }
      break;
    case urdf::Geometry::CYLINDER:
      type = "cylinder";
      {
        std::shared_ptr<const urdf::Cylinder> cylinder;
        cylinder = std::dynamic_pointer_cast<const urdf::Cylinder >(_geom);
        _sizeVals[0] = cylinder->radius * 2;
        _sizeVals[1] = cylinder->radius * 2;
        _sizeVals[2] = cylinder->length;
      }
      break;
    case urdf::Geometry::SPHERE:
      type = "sphere";
      {
        std::shared_ptr<const urdf::Sphere> sphere;
        sphere = std::dynamic_pointer_cast<const urdf::Sphere >(_geom);
        _sizeVals[0] = _sizeVals[1] = _sizeVals[2] = sphere->radius * 2;
      }
      break;
    case urdf::Geometry::MESH:
      type = "trimesh";
      {
        std::shared_ptr<const urdf::Mesh> mesh;
        mesh = std::dynamic_pointer_cast<const urdf::Mesh >(_geom);
        _sizeVals[0] = mesh->scale.x;
        _sizeVals[1] = mesh->scale.y;
        _sizeVals[2] = mesh->scale.z;
      }
      break;
    default:
      _sizeVals[0] = _sizeVals[1] = _sizeVals[2] = 0;
      sdfwarn << "Unknown body type: [" << static_cast<int>(_geom->type)
        << "] skipped in geometry\n";
      break;
  }

  return type;
}

////////////////////////////////////////////////////////////////////////////////
void PrintCollisionGroups(UrdfLinkPtr _link)
{
#ifndef URDF_GE_0P3
  sdfdbg << "COLLISION LUMPING: link: [" << _link->name << "] contains ["
    << static_cast<int>(_link->collision_groups.size())
    << "] collisions.\n";
  for (std::map<std::string,
      std::shared_ptr<std::vector<UrdfCollisionPtr > > >::iterator
      colsIt = _link->collision_groups.begin();
      colsIt != _link->collision_groups.end(); ++colsIt)
  {
    sdfdbg << "    collision_groups: [" << colsIt->first << "] has ["
      << static_cast<int>(colsIt->second->size())
      << "] Collision objects\n";
  }
#else
  sdfdbg << "COLLISION LUMPING: link: [" << _link->name << "] contains ["
    << static_cast<int>(_link->collision_array.size())
    << "] collisions.\n";
#endif
}

////////////////////////////////////////////////////////////////////////////////
urdf::Pose TransformToParentFrame(
    urdf::Pose _transformInLinkFrame, urdf::Pose _parentToLinkTransform)
{
  // transform to ignition::math::Pose3d then call TransformToParentFrame
  ignition::math::Pose3d p1 = CopyPose(_transformInLinkFrame);
  ignition::math::Pose3d p2 = CopyPose(_parentToLinkTransform);
  return CopyPose(TransformToParentFrame(p1, p2));
}

////////////////////////////////////////////////////////////////////////////////
ignition::math::Pose3d TransformToParentFrame(ignition::math::Pose3d _transformInLinkFrame,
    urdf::Pose _parentToLinkTransform)
{
  // transform to ignition::math::Pose3d then call TransformToParentFrame
  ignition::math::Pose3d p2 = CopyPose(_parentToLinkTransform);
  return TransformToParentFrame(_transformInLinkFrame, p2);
}

////////////////////////////////////////////////////////////////////////////////
ignition::math::Pose3d TransformToParentFrame(
    ignition::math::Pose3d _transformInLinkFrame,
    ignition::math::Pose3d _parentToLinkTransform)
{
  ignition::math::Pose3d transformInParentLinkFrame;
  // rotate link pose to parentLink frame
  transformInParentLinkFrame.Pos() =
    _parentToLinkTransform.Rot() * _transformInLinkFrame.Pos();
  transformInParentLinkFrame.Rot() =
    _parentToLinkTransform.Rot() * _transformInLinkFrame.Rot();
  // translate link to parentLink frame
  transformInParentLinkFrame.Pos() =
    _parentToLinkTransform.Pos() + transformInParentLinkFrame.Pos();

  return transformInParentLinkFrame;
}

/////////////////////////////////////////////////
/// reduced fixed joints: transform to parent frame
ignition::math::Pose3d inverseTransformToParentFrame(
    ignition::math::Pose3d _transformInLinkFrame,
    urdf::Pose _parentToLinkTransform)
{
  ignition::math::Pose3d transformInParentLinkFrame;
  //   rotate link pose to parentLink frame
  urdf::Rotation ri = _parentToLinkTransform.rotation.GetInverse();
  ignition::math::Quaterniond q1(ri.w, ri.x, ri.y, ri.z);
  transformInParentLinkFrame.Pos() = q1 * _transformInLinkFrame.Pos();
  urdf::Rotation r2 = _parentToLinkTransform.rotation.GetInverse();
  ignition::math::Quaterniond q3(r2.w, r2.x, r2.y, r2.z);
  transformInParentLinkFrame.Rot() = q3 * _transformInLinkFrame.Rot();
  //   translate link to parentLink frame
  transformInParentLinkFrame.Pos().X() = transformInParentLinkFrame.Pos().X()
    - _parentToLinkTransform.position.x;
  transformInParentLinkFrame.Pos().Y() = transformInParentLinkFrame.Pos().Y()
    - _parentToLinkTransform.position.y;
  transformInParentLinkFrame.Pos().Z() = transformInParentLinkFrame.Pos().Z()
    - _parentToLinkTransform.position.z;

  return transformInParentLinkFrame;
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionToParent(UrdfLinkPtr _link)
{
  /// \todo: move to header
  /// Take the link's existing list of gazebo extensions, transfer them
  /// into parent link.  Along the way, update local transforms by adding
  /// the additional transform to parent.  Also, look through all
  /// referenced link names with plugins and update references to current
  /// link to the parent link. (reduceGazeboExtensionFrameReplace())

  /// @todo: this is a very complicated module that updates the plugins
  /// based on fixed joint reduction really wish this could be a lot cleaner

  std::string linkName = _link->name;

  // update extension map with references to linkName
  // this->ListSDFExtensions();
  StringSDFExtensionPtrMap::iterator ext = g_extensions.find(linkName);
  if (ext != g_extensions.end())
  {
    sdfdbg << "  REDUCE EXTENSION: moving reference from ["
           << linkName << "] to [" << _link->getParent()->name << "]\n";

    // update reduction transform (for rays, cameras for now).
    //   FIXME: contact frames too?
    for (std::vector<SDFExtensionPtr>::iterator ge = ext->second.begin();
        ge != ext->second.end(); ++ge)
    {
      (*ge)->reductionTransform = TransformToParentFrame(
          (*ge)->reductionTransform,
          _link->parent_joint->parent_to_joint_origin_transform);
      // for sensor and projector blocks only
      ReduceSDFExtensionsTransform((*ge));
    }

    // find pointer to the existing extension with the new _link reference
    std::string parentLinkName = _link->getParent()->name;
    StringSDFExtensionPtrMap::iterator parentExt = g_extensions.find(parentLinkName);

    // if none exist, create new extension with parentLinkName
    if (parentExt == g_extensions.end())
    {
      std::vector<SDFExtensionPtr> ge;
      g_extensions.insert(std::make_pair(
            parentLinkName, ge));
      parentExt = g_extensions.find(parentLinkName);
    }

    // move sdf extensions from _link into the parent _link's extensions
    for (std::vector<SDFExtensionPtr>::iterator ge = ext->second.begin();
        ge != ext->second.end(); ++ge)
      parentExt->second.push_back(*ge);
    ext->second.clear();
  }

  // for extensions with empty reference, search and replace
  // _link name patterns within the plugin with new _link name
  // and assign the proper reduction transform for the _link name pattern
  for (StringSDFExtensionPtrMap::iterator
      sdfIt = g_extensions.begin();
      sdfIt != g_extensions.end(); ++sdfIt)
  {
    // update reduction transform (for contacts, rays, cameras for now).
    for (std::vector<SDFExtensionPtr>::iterator
        ge = sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge)
      ReduceSDFExtensionFrameReplace(*ge, _link);
  }

  // this->ListSDFExtensions();
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge,
    UrdfLinkPtr _link)
{
  std::string linkName = _link->name;
  std::string parentLinkName = _link->getParent()->name;

  // HACK: need to do this more generally, but we also need to replace
  //       all instances of _link name with new link name
  //       e.g. contact sensor refers to
  //         <collision>base_link_collision</collision>
  //         and it needs to be reparented to
  //         <collision>base_footprint_collision</collision>
  sdfdbg << "  STRING REPLACE: instances of _link name ["
        << linkName << "] with [" << parentLinkName << "]\n";
  for (std::vector<TiXmlElementPtr>::iterator blobIt =
         _ge->blobs.begin();
         blobIt != _ge->blobs.end(); ++blobIt)
  {
    std::ostringstream debugStreamIn;
    debugStreamIn << *(*blobIt);
    std::string debugBlob = debugStreamIn.str();
    sdfdbg << "        INITIAL STRING link ["
           << linkName << "]-->[" << parentLinkName << "]: ["
           << debugBlob << "]\n";

    ReduceSDFExtensionContactSensorFrameReplace(blobIt, _link);
    ReduceSDFExtensionPluginFrameReplace(blobIt, _link,
        "plugin", "bodyName", _ge->reductionTransform);
    ReduceSDFExtensionPluginFrameReplace(blobIt, _link,
        "plugin", "frameName", _ge->reductionTransform);
    ReduceSDFExtensionProjectorFrameReplace(blobIt, _link);
    ReduceSDFExtensionGripperFrameReplace(blobIt, _link);
    ReduceSDFExtensionJointFrameReplace(blobIt, _link);

    std::ostringstream debugStreamOut;
    debugStreamOut << *(*blobIt);
  }
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionsTransform(SDFExtensionPtr _ge)
{
  for (std::vector<TiXmlElementPtr>::iterator blobIt =
         _ge->blobs.begin();
         blobIt != _ge->blobs.end(); ++blobIt)
  {
    /// @todo make sure we are not missing any additional transform reductions
    ReduceSDFExtensionSensorTransformReduction(blobIt,
        _ge->reductionTransform);
    ReduceSDFExtensionProjectorTransformReduction(blobIt,
        _ge->reductionTransform);
  }
}

////////////////////////////////////////////////////////////////////////////////
void URDF2SDF::ListSDFExtensions()
{
  for (StringSDFExtensionPtrMap::iterator
      sdfIt = g_extensions.begin();
      sdfIt != g_extensions.end(); ++sdfIt)
  {
    int extCount = 0;
    for (std::vector<SDFExtensionPtr>::iterator ge = sdfIt->second.begin();
        ge != sdfIt->second.end(); ++ge)
    {
      if (!(*ge)->blobs.empty())
      {
        sdfdbg <<  "  PRINTING [" << static_cast<int>((*ge)->blobs.size())
          << "] BLOBS for extension [" << ++extCount
          << "] referencing [" << sdfIt->first << "]\n";
        for (std::vector<TiXmlElementPtr>::iterator
            blobIt = (*ge)->blobs.begin();
            blobIt != (*ge)->blobs.end(); ++blobIt)
        {
          std::ostringstream streamIn;
          streamIn << *(*blobIt);
          sdfdbg << "    BLOB: [" << streamIn.str() << "]\n";
        }
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void URDF2SDF::ListSDFExtensions(const std::string &_reference)
{
  for (StringSDFExtensionPtrMap::iterator
      sdfIt = g_extensions.begin();
      sdfIt != g_extensions.end(); ++sdfIt)
  {
    if (sdfIt->first == _reference)
    {
      sdfdbg <<  "  PRINTING [" << static_cast<int>(sdfIt->second.size())
        << "] extensions referencing [" << _reference << "]\n";
      for (std::vector<SDFExtensionPtr>::iterator
          ge = sdfIt->second.begin(); ge != sdfIt->second.end(); ++ge)
      {
        for (std::vector<TiXmlElementPtr>::iterator
            blobIt = (*ge)->blobs.begin();
            blobIt != (*ge)->blobs.end(); ++blobIt)
        {
          std::ostringstream streamIn;
          streamIn << *(*blobIt);
          sdfdbg << "    BLOB: [" << streamIn.str() << "]\n";
        }
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void CreateSDF(TiXmlElement *_root,
    ConstUrdfLinkPtr _link,
    const ignition::math::Pose3d &_transform)
{
  ignition::math::Pose3d _currentTransform = _transform;

  // must have an <inertial> block and cannot have zero mass.
  //  allow det(I) == zero, in the case of point mass geoms.
  // @todo:  keyword "world" should be a constant defined somewhere else
  if (_link->name != "world" &&
      ((!_link->inertial) || ignition::math::equal(_link->inertial->mass, 0.0)))
  {
    if (!_link->child_links.empty())
      sdfdbg << "urdf2sdf: link[" << _link->name
        << "] has no inertia, ["
        << static_cast<int>(_link->child_links.size())
        << "] children links ignored.\n";

    if (!_link->child_joints.empty())
      sdfdbg << "urdf2sdf: link[" << _link->name
        << "] has no inertia, ["
        << static_cast<int>(_link->child_links.size())
        << "] children joints ignored.\n";

    if (_link->parent_joint)
      sdfdbg << "urdf2sdf: link[" << _link->name
        << "] has no inertia, "
        << "parent joint [" << _link->parent_joint->name
        << "] ignored.\n";

    sdfdbg << "urdf2sdf: link[" << _link->name
      << "] has no inertia, not modeled in sdf\n";
    return;
  }

  // create <body:...> block for non fixed joint attached bodies
  if ((_link->getParent() && _link->getParent()->name == "world") ||
      !g_reduceFixedJoints ||
      (!_link->parent_joint ||
       !FixedJointShouldBeReduced(_link->parent_joint)))
    CreateLink(_root, _link, _currentTransform);

  // recurse into children
  for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i)
    CreateSDF(_root, _link->child_links[i], _currentTransform);
}

////////////////////////////////////////////////////////////////////////////////
ignition::math::Pose3d CopyPose(urdf::Pose _pose)
{
  ignition::math::Pose3d p;
  p.Pos().X() = _pose.position.x;
  p.Pos().Y() = _pose.position.y;
  p.Pos().Z() = _pose.position.z;
  p.Rot().X() = _pose.rotation.x;
  p.Rot().Y() = _pose.rotation.y;
  p.Rot().Z() = _pose.rotation.z;
  p.Rot().W() = _pose.rotation.w;
  return p;
}

////////////////////////////////////////////////////////////////////////////////
urdf::Pose CopyPose(ignition::math::Pose3d _pose)
{
  urdf::Pose p;
  p.position.x = _pose.Pos().X();
  p.position.y = _pose.Pos().Y();
  p.position.z = _pose.Pos().Z();
  p.rotation.x = _pose.Rot().X();
  p.rotation.y = _pose.Rot().Y();
  p.rotation.z = _pose.Rot().Z();
  p.rotation.w = _pose.Rot().W();
  return p;
}

////////////////////////////////////////////////////////////////////////////////
void CreateLink(TiXmlElement *_root,
    ConstUrdfLinkPtr _link,
    ignition::math::Pose3d &_currentTransform)
{
  // create new body
  TiXmlElement *elem     = new TiXmlElement("link");

  // set body name
  elem->SetAttribute("name", _link->name);

  // compute global transform
  ignition::math::Pose3d localTransform;
  // this is the transform from parent link to current _link
  // this transform does not exist for the root link
  if (_link->parent_joint)
  {
    localTransform = CopyPose(
        _link->parent_joint->parent_to_joint_origin_transform);
    _currentTransform = localTransform * _currentTransform;
  }
  else
    sdfdbg << "[" << _link->name << "] has no parent joint\n";

  // create origin tag for this element
  AddTransform(elem, _currentTransform);

  // create new inerial block
  CreateInertial(elem, _link);

  // create new collision block
  CreateCollisions(elem, _link);

  // create new visual block
  CreateVisuals(elem, _link);

  // copy sdf extensions data
  InsertSDFExtensionLink(elem, _link->name);

  // add body to document
  _root->LinkEndChild(elem);

  // make a <joint:...> block
  CreateJoint(_root, _link, _currentTransform);
}

////////////////////////////////////////////////////////////////////////////////
void CreateCollisions(TiXmlElement* _elem,
    ConstUrdfLinkPtr _link)
{
  // loop through all collisions in
  //   collision_array (urdf 0.3.x)
  //   collision_groups (urdf 0.2.x)
  // and create collision sdf blocks.
  // Note, well as additional collision from
  //   lumped meshes (fixed joint reduction)
#ifndef URDF_GE_0P3
  for (std::map<std::string,
      std::shared_ptr<std::vector<UrdfCollisionPtr> > >::const_iterator
      collisionsIt = _link->collision_groups.begin();
      collisionsIt != _link->collision_groups.end(); ++collisionsIt)
  {
    unsigned int defaultMeshCount = 0;
    unsigned int groupMeshCount = 0;
    unsigned int lumpMeshCount = 0;
    // loop through collisions in each group
    for (std::vector<UrdfCollisionPtr>::iterator
        collision = collisionsIt->second->begin();
        collision != collisionsIt->second->end();
        ++collision)
    {
      if (collisionsIt->first == "default")
      {
        sdfdbg << "creating default collision for link [" << _link->name
               << "]";

        std::string collisionPrefix = _link->name;

        if (defaultMeshCount > 0)
        {
          // append _[meshCount] to link name for additional collisions
          std::ostringstream collisionNameStream;
          collisionNameStream << collisionPrefix << "_" << defaultMeshCount;
          collisionPrefix = collisionNameStream.str();
        }

        /* make a <collision> block */
        CreateCollision(_elem, _link, *collision, collisionPrefix);

        // only 1 default mesh
        ++defaultMeshCount;
      }
      else if (collisionsIt->first.find(g_lumpPrefix) == 0)
      {
        // if collision name starts with g_lumpPrefix, pass through
        //   original parent link name
        sdfdbg << "creating lump collision [" << collisionsIt->first
               << "] for link [" << _link->name << "].\n";
        /// collisionPrefix is the original name before lumping
        std::string collisionPrefix = collisionsIt->first.substr(6);

        if (lumpMeshCount > 0)
        {
          // append _[meshCount] to link name for additional collisions
          std::ostringstream collisionNameStream;
          collisionNameStream << collisionPrefix << "_" << lumpMeshCount;
          collisionPrefix = collisionNameStream.str();
        }

        CreateCollision(_elem, _link, *collision, collisionPrefix);
        ++lumpMeshCount;
      }
      else
      {
        sdfdbg << "adding collisions from collision group ["
              << collisionsIt->first << "]\n";

        std::string collisionPrefix = _link->name + std::string("_") +
          collisionsIt->first;

        if (groupMeshCount > 0)
        {
          // append _[meshCount] to _link name for additional collisions
          std::ostringstream collisionNameStream;
          collisionNameStream << collisionPrefix << "_" << groupMeshCount;
          collisionPrefix = collisionNameStream.str();
        }

        CreateCollision(_elem, _link, *collision, collisionPrefix);
        ++groupMeshCount;
      }
    }
  }
#else
  unsigned int collisionCount = 0;
  for (std::vector<UrdfCollisionPtr>::const_iterator
      collision = _link->collision_array.begin();
      collision != _link->collision_array.end();
      ++collision)
  {
    sdfdbg << "creating collision for link [" << _link->name
           << "] collision [" << (*collision)->name << "]\n";

    // collision sdf has a name if it was lumped/reduced
    // otherwise, use the link name
    std::string collisionName = (*collision)->name;
    if (collisionName.empty())
      collisionName = _link->name;

    // add _collision extension
    collisionName = collisionName + g_collisionExt;

    if (collisionCount > 0)
    {
      std::ostringstream collisionNameStream;
      collisionNameStream << collisionName
                          << "_" << collisionCount;
      collisionName = collisionNameStream.str();
    }

    // make a <collision> block
    CreateCollision(_elem, _link, *collision, collisionName);

    ++collisionCount;
  }
#endif
}

////////////////////////////////////////////////////////////////////////////////
void CreateVisuals(TiXmlElement* _elem,
    ConstUrdfLinkPtr _link)
{
  // loop through all visuals in
  //   visual_array (urdf 0.3.x)
  //   visual_groups (urdf 0.2.x)
  // and create visual sdf blocks.
  // Note, well as additional visual from
  //   lumped meshes (fixed joint reduction)
#ifndef URDF_GE_0P3
  for (std::map<std::string,
      std::shared_ptr<std::vector<UrdfVisualPtr> > >::const_iterator
      visualsIt = _link->visual_groups.begin();
      visualsIt != _link->visual_groups.end(); ++visualsIt)
  {
    unsigned int defaultMeshCount = 0;
    unsigned int groupMeshCount = 0;
    unsigned int lumpMeshCount = 0;
    // loop through all visuals in this group
    for (std::vector<UrdfVisualPtr>::iterator
        visual = visualsIt->second->begin();
        visual != visualsIt->second->end();
        ++visual)
    {
      if (visualsIt->first == "default")
      {
        sdfdbg << "creating default visual for link [" << _link->name
               << "]";

        std::string visualPrefix = _link->name;

        if (defaultMeshCount > 0)
        {
          // append _[meshCount] to _link name for additional visuals
          std::ostringstream visualNameStream;
          visualNameStream << visualPrefix << "_" << defaultMeshCount;
          visualPrefix = visualNameStream.str();
        }

        // create a <visual> block
        CreateVisual(_elem, _link, *visual, visualPrefix);

        // only 1 default mesh
        ++defaultMeshCount;
      }
      else if (visualsIt->first.find(g_lumpPrefix) == 0)
      {
        // if visual name starts with g_lumpPrefix, pass through
        //   original parent link name
        sdfdbg << "creating lump visual [" << visualsIt->first
               << "] for link [" << _link->name << "].\n";
        /// visualPrefix is the original name before lumping
        std::string visualPrefix = visualsIt->first.substr(6);

        if (lumpMeshCount > 0)
        {
          // append _[meshCount] to _link name for additional visuals
          std::ostringstream visualNameStream;
          visualNameStream << visualPrefix << "_" << lumpMeshCount;
          visualPrefix = visualNameStream.str();
        }

        CreateVisual(_elem, _link, *visual, visualPrefix);
        ++lumpMeshCount;
      }
      else
      {
        sdfdbg << "adding visuals from visual group ["
              << visualsIt->first << "]\n";

        std::string visualPrefix = _link->name + std::string("_") +
          visualsIt->first;

        if (groupMeshCount > 0)
        {
          // append _[meshCount] to _link name for additional visuals
          std::ostringstream visualNameStream;
          visualNameStream << visualPrefix << "_" << groupMeshCount;
          visualPrefix = visualNameStream.str();
        }

        CreateVisual(_elem, _link, *visual, visualPrefix);
        ++groupMeshCount;
      }
    }
  }
#else
  unsigned int visualCount = 0;
  for (std::vector<UrdfVisualPtr>::const_iterator
      visual = _link->visual_array.begin();
      visual != _link->visual_array.end();
      ++visual)
  {
    sdfdbg << "creating visual for link [" << _link->name
           << "] visual [" << (*visual)->name << "]\n";

    // visual sdf has a name if it was lumped/reduced
    // otherwise, use the link name
    std::string visualName = (*visual)->name;
    if (visualName.empty())
      visualName = _link->name;

    // add _visual extension
    visualName = visualName + g_visualExt;

    if (visualCount > 0)
    {
      std::ostringstream visualNameStream;
      visualNameStream << visualName
                          << "_" << visualCount;
      visualName = visualNameStream.str();
    }

    // make a <visual> block
    CreateVisual(_elem, _link, *visual, visualName);

    ++visualCount;
  }
#endif
}

////////////////////////////////////////////////////////////////////////////////
void CreateInertial(TiXmlElement *_elem,
    ConstUrdfLinkPtr _link)
{
  TiXmlElement *inertial = new TiXmlElement("inertial");

  // set mass properties
  // check and print a warning message
  double roll, pitch, yaw;
  _link->inertial->origin.rotation.getRPY(roll, pitch, yaw);

  /// add pose
  ignition::math::Pose3d pose = CopyPose(_link->inertial->origin);
  AddTransform(inertial, pose);

  // add mass
  AddKeyValue(inertial, "mass",
      Values2str(1, &_link->inertial->mass));

  // add inertia (ixx, ixy, ixz, iyy, iyz, izz)
  TiXmlElement *inertia = new TiXmlElement("inertia");
  AddKeyValue(inertia, "ixx",
      Values2str(1, &_link->inertial->ixx));
  AddKeyValue(inertia, "ixy",
      Values2str(1, &_link->inertial->ixy));
  AddKeyValue(inertia, "ixz",
      Values2str(1, &_link->inertial->ixz));
  AddKeyValue(inertia, "iyy",
      Values2str(1, &_link->inertial->iyy));
  AddKeyValue(inertia, "iyz",
      Values2str(1, &_link->inertial->iyz));
  AddKeyValue(inertia, "izz",
      Values2str(1, &_link->inertial->izz));
  inertial->LinkEndChild(inertia);

  _elem->LinkEndChild(inertial);
}

////////////////////////////////////////////////////////////////////////////////
void CreateJoint(TiXmlElement *_root,
    ConstUrdfLinkPtr _link,
    ignition::math::Pose3d &_currentTransform)
{
  // compute the joint tag
  std::string jtype;
  jtype.clear();
  if (_link->parent_joint != NULL)
  {
    switch (_link->parent_joint->type)
    {
      case urdf::Joint::CONTINUOUS:
      case urdf::Joint::REVOLUTE:
        jtype = "revolute";
        break;
      case urdf::Joint::PRISMATIC:
        jtype = "prismatic";
        break;
      case urdf::Joint::FLOATING:
      case urdf::Joint::PLANAR:
        break;
      case urdf::Joint::FIXED:
        jtype = "fixed";
        break;
      default:
        sdfwarn << "Unknown joint type: [" << static_cast<int>(_link->parent_joint->type)
          << "] in link [" << _link->name << "]\n";
        break;
    }
  }

  // skip if joint type is fixed and we are not faking it with a hinge,
  //   skip/return with the exception of root link being world,
  //   because there's no lumping there
  if (_link->getParent() && _link->getParent()->name != "world"
      && FixedJointShouldBeReduced(_link->parent_joint)
      && g_reduceFixedJoints) return;

  if (!jtype.empty())
  {
    TiXmlElement *joint = new TiXmlElement("joint");
    if (jtype == "fixed")
      joint->SetAttribute("type", "revolute");
    else
      joint->SetAttribute("type", jtype);
    joint->SetAttribute("name", _link->parent_joint->name);
    AddKeyValue(joint, "child", _link->name);
    AddKeyValue(joint, "parent", _link->getParent()->name);

    TiXmlElement *jointAxis = new TiXmlElement("axis");
    TiXmlElement *jointAxisLimit = new TiXmlElement("limit");
    TiXmlElement *jointAxisDynamics = new TiXmlElement("dynamics");
    if (jtype == "fixed")
    {
      AddKeyValue(jointAxisLimit, "lower", "0");
      AddKeyValue(jointAxisLimit, "upper", "0");
      AddKeyValue(jointAxisDynamics, "damping", "0");
      AddKeyValue(jointAxisDynamics, "friction", "0");
    }
    else
    {
      ignition::math::Vector3d rotatedJointAxis =
        _currentTransform.Rot().RotateVector(
            ignition::math::Vector3d(_link->parent_joint->axis.x,
              _link->parent_joint->axis.y,
              _link->parent_joint->axis.z));
      double rotatedJointAxisArray[3] =
      { rotatedJointAxis.X(), rotatedJointAxis.Y(), rotatedJointAxis.Z() };
      AddKeyValue(jointAxis, "xyz",
          Values2str(3, rotatedJointAxisArray));
      if (_link->parent_joint->dynamics)
      {
        AddKeyValue(jointAxisDynamics, "damping",
            Values2str(1, &_link->parent_joint->dynamics->damping));
        AddKeyValue(jointAxisDynamics, "friction",
            Values2str(1, &_link->parent_joint->dynamics->friction));
      }

      if (g_enforceLimits && _link->parent_joint->limits)
      {
        if (jtype == "slider")
        {
          AddKeyValue(jointAxisLimit, "lower",
              Values2str(1, &_link->parent_joint->limits->lower));
          AddKeyValue(jointAxisLimit, "upper",
              Values2str(1, &_link->parent_joint->limits->upper));
          AddKeyValue(jointAxisLimit, "effort",
              Values2str(1, &_link->parent_joint->limits->effort));
          AddKeyValue(jointAxisLimit, "velocity",
              Values2str(1, &_link->parent_joint->limits->velocity));
        }
        else if (_link->parent_joint->type != urdf::Joint::CONTINUOUS)
        {
          double *lowstop  = &_link->parent_joint->limits->lower;
          double *highstop = &_link->parent_joint->limits->upper;
          // enforce ode bounds, this will need to be fixed
          if (*lowstop > *highstop)
          {
            sdfwarn << "urdf2sdf: revolute joint ["
              << _link->parent_joint->name
              << "] with limits: lowStop[" << *lowstop
              << "] > highStop[" << highstop
              << "], switching the two.\n";
            double tmp = *lowstop;
            *lowstop = *highstop;
            *highstop = tmp;
          }
          AddKeyValue(jointAxisLimit, "lower",
              Values2str(1, &_link->parent_joint->limits->lower));
          AddKeyValue(jointAxisLimit, "upper",
              Values2str(1, &_link->parent_joint->limits->upper));
          AddKeyValue(jointAxisLimit, "effort",
              Values2str(1, &_link->parent_joint->limits->effort));
          AddKeyValue(jointAxisLimit, "velocity",
              Values2str(1, &_link->parent_joint->limits->velocity));
        }
      }
    }
    jointAxis->LinkEndChild(jointAxisLimit);
    jointAxis->LinkEndChild(jointAxisDynamics);
    joint->LinkEndChild(jointAxis);

    // copy sdf extensions data
    InsertSDFExtensionJoint(joint, _link->parent_joint->name);

    // add joint to document
    _root->LinkEndChild(joint);
  }
}

////////////////////////////////////////////////////////////////////////////////
void CreateCollision(TiXmlElement* _elem, ConstUrdfLinkPtr _link,
    UrdfCollisionPtr _collision, const std::string &_oldLinkName)
{
  // begin create geometry node, skip if no collision specified
  TiXmlElement *sdfCollision = new TiXmlElement("collision");

  // std::cerr << "CreateCollision link [" << _link->name
  //           << "] old [" << _oldLinkName
  //           << "]\n";
  // set its name, if lumped, add original link name
  // for meshes in an original mesh, it's likely
  // _link->name + mesh count
  if (_oldLinkName.compare(0, _link->name.size(), _link->name) == 0 ||
      _oldLinkName.empty())
  {
    sdfCollision->SetAttribute("name", _oldLinkName);
  }
  else
  {
    sdfCollision->SetAttribute("name", _link->name
        + g_lumpPrefix + _oldLinkName);
  }

  // std::cerr << "collision [" << sdfCollision->Attribute("name") << "]\n";

  // set transform
  double pose[6];
  pose[0] = _collision->origin.position.x;
  pose[1] = _collision->origin.position.y;
  pose[2] = _collision->origin.position.z;
  _collision->origin.rotation.getRPY(pose[3], pose[4], pose[5]);
  AddKeyValue(sdfCollision, "pose", Values2str(6, pose));

  // add geometry block
  if (!_collision || !_collision->geometry)
  {
    sdfdbg << "urdf2sdf: collision of link [" << _link->name
           << "] has no <geometry>.\n";
  }
  else
  {
    CreateGeometry(sdfCollision, _collision->geometry);
  }

  // set additional data from extensions
  InsertSDFExtensionCollision(sdfCollision, _link->name);

  // add geometry to body
  _elem->LinkEndChild(sdfCollision);
}

////////////////////////////////////////////////////////////////////////////////
void CreateVisual(TiXmlElement *_elem, ConstUrdfLinkPtr _link,
    UrdfVisualPtr _visual, const std::string &_oldLinkName)
{
  // begin create sdf visual node
  TiXmlElement *sdfVisual = new TiXmlElement("visual");

  // set its name
  if (_oldLinkName.compare(0, _link->name.size(), _link->name) == 0 ||
      _oldLinkName.empty())
  {
    sdfVisual->SetAttribute("name", _oldLinkName);
  }
  else
  {
    sdfVisual->SetAttribute("name", _link->name
        + g_lumpPrefix + _oldLinkName);
  }

  // add the visualisation transfrom
  double pose[6];
  pose[0] = _visual->origin.position.x;
  pose[1] = _visual->origin.position.y;
  pose[2] = _visual->origin.position.z;
  _visual->origin.rotation.getRPY(pose[3], pose[4], pose[5]);
  AddKeyValue(sdfVisual, "pose", Values2str(6, pose));

  // insert geometry
  if (!_visual || !_visual->geometry)
  {
    sdfdbg << "urdf2sdf: visual of link [" << _link->name
           << "] has no <geometry>.\n";
  }
  else
  {
    CreateGeometry(sdfVisual, _visual->geometry);
  }

  // set additional data from extensions
  InsertSDFExtensionVisual(sdfVisual, _link->name);

  // end create _visual node
  _elem->LinkEndChild(sdfVisual);
}

////////////////////////////////////////////////////////////////////////////////
TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr,
    bool _enforceLimits)
{
  g_enforceLimits = _enforceLimits;

  // Create a RobotModel from string
  std::shared_ptr<urdf::ModelInterface> robotModel =
    urdf::parseURDF(_urdfStr.c_str());

  // an xml object to hold the xml result
  TiXmlDocument sdfXmlOut;

  if (!robotModel)
  {
    sdferr << "Unable to call parseURDF on robot model\n";
    return sdfXmlOut;
  }

  // create root element and define needed namespaces
  TiXmlElement *robot = new TiXmlElement("model");

  // set model name to urdf robot name if not specified
  robot->SetAttribute("name", robotModel->getName());

  // initialize transform for the model, urdf is recursive,
  // while sdf defines all links relative to model frame
  ignition::math::Pose3d transform;

  // parse sdf extension
  TiXmlDocument urdfXml;
  urdfXml.Parse(_urdfStr.c_str());
  g_extensions.clear();
  this->ParseSDFExtension(urdfXml);

  // Parse robot pose
  ParseRobotOrigin(urdfXml);

  ConstUrdfLinkPtr rootLink = robotModel->getRoot();

  // Fixed Joint Reduction
  // if link connects to parent via fixed joint, lump down and remove link
  // set reduceFixedJoints to false will replace fixed joints with
  // zero limit revolute joints, otherwise, we reduce it down to its
  // parent link recursively
  // using the disabledFixedJointLumping option is possible to disable
  // fixed joint lumping only for selected joints
  if (g_reduceFixedJoints)
    ReduceFixedJoints(robot,
        (std::const_pointer_cast< urdf::Link >(rootLink)));

  if (rootLink->name == "world")
  {
    // convert all children link
    for (std::vector<UrdfLinkPtr>::const_iterator
        child = rootLink->child_links.begin();
        child != rootLink->child_links.end(); ++child)
      CreateSDF(robot, (*child), transform);
  }
  else
  {
    // convert, starting from root link
    CreateSDF(robot, rootLink, transform);
  }

  // insert the extensions without reference into <robot> root level
  InsertSDFExtensionRobot(robot);

  InsertRobotOrigin(robot);

  // add robot to sdfXmlOut
  TiXmlElement *sdf = new TiXmlElement("sdf");

  // URDF is compatible with version 1.4. The automatic conversion script
  // will up-convert URDF to SDF.
  sdf->SetAttribute("version", "1.4");

  sdf->LinkEndChild(robot);
  sdfXmlOut.LinkEndChild(sdf);

  // debug
  // sdfXmlOut.Print();

  return sdfXmlOut;
}

////////////////////////////////////////////////////////////////////////////////
TiXmlDocument URDF2SDF::InitModelDoc(TiXmlDocument* _xmlDoc)
{
  std::ostringstream stream;
  stream << *_xmlDoc;
  std::string urdfStr = stream.str();
  return InitModelString(urdfStr);
}

////////////////////////////////////////////////////////////////////////////////
TiXmlDocument URDF2SDF::InitModelFile(const std::string &_filename)
{
  TiXmlDocument xmlDoc;
  if (xmlDoc.LoadFile(_filename))
  {
    return this->InitModelDoc(&xmlDoc);
  }
  else
    sdferr << "Unable to load file[" << _filename << "].\n";

  return xmlDoc;
}

////////////////////////////////////////////////////////////////////////////////
bool FixedJointShouldBeReduced(std::shared_ptr<urdf::Joint> _jnt)
{
    // A joint should be lumped only if its type is fixed and
    // the disabledFixedJointLumping joint option is not set
    return (_jnt->type == urdf::Joint::FIXED &&
              (g_fixedJointsNotReduced.find(_jnt->name) ==
                 g_fixedJointsNotReduced.end()) );
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionSensorTransformReduction(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    ignition::math::Pose3d _reductionTransform)
{
  // overwrite <xyz> and <rpy> if they exist
  if ((*_blobIt)->ValueStr() == "sensor")
  {
    // parse it and add/replace the reduction transform
    // find first instance of xyz and rpy, replace with reduction transform

    // debug print
    // for (TiXmlNode* elIt = (*_blobIt)->FirstChild();
    //      elIt; elIt = elIt->NextSibling())
    // {
    //   std::ostringstream streamIn;
    //   streamIn << *elIt;
    //   sdfdbg << "    " << streamIn << "\n";
    // }

    {
      TiXmlNode* oldPoseKey = (*_blobIt)->FirstChild("pose");
      /// @todo: FIXME:  we should read xyz, rpy and aggregate it to
      /// reductionTransform instead of just throwing the info away.
      if (oldPoseKey)
        (*_blobIt)->RemoveChild(oldPoseKey);
    }

    // convert reductionTransform to values
    urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
        _reductionTransform.Pos().Y(),
        _reductionTransform.Pos().Z());
    urdf::Rotation reductionQ(_reductionTransform.Rot().X(),
        _reductionTransform.Rot().Y(),
        _reductionTransform.Rot().Z(),
        _reductionTransform.Rot().W());

    urdf::Vector3 reductionRpy;
    reductionQ.getRPY(reductionRpy.x, reductionRpy.y, reductionRpy.z);

    // output updated pose to text
    std::ostringstream poseStream;
    poseStream << reductionXyz.x << " " << reductionXyz.y
      << " " << reductionXyz.z << " " << reductionRpy.x
      << " " << reductionRpy.y << " " << reductionRpy.z;
    TiXmlText* poseTxt = new TiXmlText(poseStream.str());

    TiXmlElement* poseKey = new TiXmlElement("pose");
    poseKey->LinkEndChild(poseTxt);

    (*_blobIt)->LinkEndChild(poseKey);
  }
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionProjectorTransformReduction(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    ignition::math::Pose3d _reductionTransform)
{
  // overwrite <pose> (xyz/rpy) if it exists
  if ((*_blobIt)->ValueStr() == "projector")
  {
    // parse it and add/replace the reduction transform
    // find first instance of xyz and rpy, replace with reduction transform
    //
    // for (TiXmlNode* elIt = (*_blobIt)->FirstChild();
    // elIt; elIt = elIt->NextSibling())
    // {
    //   std::ostringstream streamIn;
    //   streamIn << *elIt;
    //   sdfdbg << "    " << streamIn << "\n";
    // }

    // should read <pose>...</pose> and agregate reductionTransform
    TiXmlNode* poseKey = (*_blobIt)->FirstChild("pose");
    // read pose and save it

    // remove the tag for now
    if (poseKey) (*_blobIt)->RemoveChild(poseKey);

    // convert reductionTransform to values
    urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
        _reductionTransform.Pos().Y(),
        _reductionTransform.Pos().Z());
    urdf::Rotation reductionQ(_reductionTransform.Rot().X(),
        _reductionTransform.Rot().Y(),
        _reductionTransform.Rot().Z(),
        _reductionTransform.Rot().W());

    urdf::Vector3 reductionRpy;
    reductionQ.getRPY(reductionRpy.x, reductionRpy.y, reductionRpy.z);

    // output updated pose to text
    std::ostringstream poseStream;
    poseStream << reductionXyz.x << " " << reductionXyz.y
      << " " << reductionXyz.z << " " << reductionRpy.x
      << " " << reductionRpy.y << " " << reductionRpy.z;
    TiXmlText* poseTxt = new TiXmlText(poseStream.str());

    poseKey = new TiXmlElement("pose");
    poseKey->LinkEndChild(poseTxt);

    (*_blobIt)->LinkEndChild(poseKey);
  }
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionContactSensorFrameReplace(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    UrdfLinkPtr _link)
{
  std::string linkName = _link->name;
  std::string parentLinkName = _link->getParent()->name;
  if ((*_blobIt)->ValueStr() == "sensor")
  {
    // parse it and add/replace the reduction transform
    // find first instance of xyz and rpy, replace with reduction transform
    TiXmlNode* contact = (*_blobIt)->FirstChild("contact");
    if (contact)
    {
      TiXmlNode* collision = contact->FirstChild("collision");
      if (collision)
      {
        if (GetKeyValueAsString(collision->ToElement()) ==
            linkName + g_collisionExt)
        {
          contact->RemoveChild(collision);
          TiXmlElement* collisionNameKey = new TiXmlElement("collision");
          std::ostringstream collisionNameStream;
          collisionNameStream << parentLinkName << g_collisionExt
            << "_" << linkName;
          TiXmlText* collisionNameTxt = new TiXmlText(
              collisionNameStream.str());
          collisionNameKey->LinkEndChild(collisionNameTxt);
          contact->LinkEndChild(collisionNameKey);
        }
        // @todo: FIXME: chagning contact sensor's contact collision
        //   should trigger a update in sensor offset as well.
        //   But first we need to implement offsets in contact sensors
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionPluginFrameReplace(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    UrdfLinkPtr _link,
    const std::string &_pluginName, const std::string &_elementName,
    ignition::math::Pose3d _reductionTransform)
{
  std::string linkName = _link->name;
  std::string parentLinkName = _link->getParent()->name;
  if ((*_blobIt)->ValueStr() == _pluginName)
  {
    // replace element containing _link names to parent link names
    // find first instance of xyz and rpy, replace with reduction transform
    TiXmlNode* elementNode = (*_blobIt)->FirstChild(_elementName);
    if (elementNode)
    {
      if (GetKeyValueAsString(elementNode->ToElement()) == linkName)
      {
        (*_blobIt)->RemoveChild(elementNode);
        TiXmlElement* bodyNameKey = new TiXmlElement(_elementName);
        std::ostringstream bodyNameStream;
        bodyNameStream << parentLinkName;
        TiXmlText* bodyNameTxt = new TiXmlText(bodyNameStream.str());
        bodyNameKey->LinkEndChild(bodyNameTxt);
        (*_blobIt)->LinkEndChild(bodyNameKey);
        /// @todo update transforms for this sdf plugin too

        // look for offset transforms, add reduction transform
        TiXmlNode* xyzKey = (*_blobIt)->FirstChild("xyzOffset");
        if (xyzKey)
        {
          urdf::Vector3 v1 = ParseVector3(xyzKey);
          _reductionTransform.Pos() =
            ignition::math::Vector3d(v1.x, v1.y, v1.z);
          // remove xyzOffset and rpyOffset
          (*_blobIt)->RemoveChild(xyzKey);
        }
        TiXmlNode* rpyKey = (*_blobIt)->FirstChild("rpyOffset");
        if (rpyKey)
        {
          urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0);
          _reductionTransform.Rot() =
            ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z);
          // remove xyzOffset and rpyOffset
          (*_blobIt)->RemoveChild(rpyKey);
        }

        // pass through the parent transform from fixed joint reduction
        _reductionTransform = inverseTransformToParentFrame(_reductionTransform,
            _link->parent_joint->parent_to_joint_origin_transform);

        // create new offset xml blocks
        xyzKey = new TiXmlElement("xyzOffset");
        rpyKey = new TiXmlElement("rpyOffset");

        // create new offset xml blocks
        urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
            _reductionTransform.Pos().Y(),
            _reductionTransform.Pos().Z());
        urdf::Rotation reductionQ(_reductionTransform.Rot().X(),
            _reductionTransform.Rot().Y(), _reductionTransform.Rot().Z(),
            _reductionTransform.Rot().W());

        std::ostringstream xyzStream, rpyStream;
        xyzStream << reductionXyz.x << " " << reductionXyz.y << " "
          << reductionXyz.z;
        urdf::Vector3 reductionRpy;
        reductionQ.getRPY(reductionRpy.x, reductionRpy.y, reductionRpy.z);
        rpyStream << reductionRpy.x << " " << reductionRpy.y << " "
          << reductionRpy.z;

        TiXmlText* xyzTxt = new TiXmlText(xyzStream.str());
        TiXmlText* rpyTxt = new TiXmlText(rpyStream.str());

        xyzKey->LinkEndChild(xyzTxt);
        rpyKey->LinkEndChild(rpyTxt);

        (*_blobIt)->LinkEndChild(xyzKey);
        (*_blobIt)->LinkEndChild(rpyKey);
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionProjectorFrameReplace(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    UrdfLinkPtr _link)
{
  std::string linkName = _link->name;
  std::string parentLinkName = _link->getParent()->name;

  // updates _link reference for <projector> inside of
  // projector plugins
  // update from <projector>MyLinkName/MyProjectorName</projector>
  // to <projector>NewLinkName/MyProjectorName</projector>
  TiXmlNode* projectorElem = (*_blobIt)->FirstChild("projector");
  {
    if (projectorElem)
    {
      std::string projectorName =  GetKeyValueAsString(
          projectorElem->ToElement());
      // extract projector _link name and projector name
      size_t pos = projectorName.find("/");
      if (pos == std::string::npos)
      {
        sdferr << "no slash in projector reference tag [" << projectorName
          << "], expecting linkName/projector_name.\n";
      }
      else
      {
        std::string projectorLinkName = projectorName.substr(0, pos);

        if (projectorLinkName == linkName)
        {
          // do the replacement
          projectorName = parentLinkName + "/" +
            projectorName.substr(pos+1, projectorName.size());

          (*_blobIt)->RemoveChild(projectorElem);
          TiXmlElement *bodyNameKey = new TiXmlElement("projector");
          std::ostringstream bodyNameStream;
          bodyNameStream << projectorName;
          TiXmlText *bodyNameTxt = new TiXmlText(bodyNameStream.str());
          bodyNameKey->LinkEndChild(bodyNameTxt);
          (*_blobIt)->LinkEndChild(bodyNameKey);
        }
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionGripperFrameReplace(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    UrdfLinkPtr _link)
{
  std::string linkName = _link->name;
  std::string parentLinkName = _link->getParent()->name;

  if ((*_blobIt)->ValueStr() == "gripper")
  {
    TiXmlNode* gripperLink = (*_blobIt)->FirstChild("gripper_link");
    if (gripperLink)
    {
      if (GetKeyValueAsString(gripperLink->ToElement()) == linkName)
      {
        (*_blobIt)->RemoveChild(gripperLink);
        TiXmlElement* bodyNameKey = new TiXmlElement("gripper_link");
        std::ostringstream bodyNameStream;
        bodyNameStream << parentLinkName;
        TiXmlText* bodyNameTxt = new TiXmlText(bodyNameStream.str());
        bodyNameKey->LinkEndChild(bodyNameTxt);
        (*_blobIt)->LinkEndChild(bodyNameKey);
      }
    }
    TiXmlNode* palmLink = (*_blobIt)->FirstChild("palm_link");
    if (palmLink)
    {
      if (GetKeyValueAsString(palmLink->ToElement()) == linkName)
      {
        (*_blobIt)->RemoveChild(palmLink);
        TiXmlElement* bodyNameKey = new TiXmlElement("palm_link");
        std::ostringstream bodyNameStream;
        bodyNameStream << parentLinkName;
        TiXmlText* bodyNameTxt = new TiXmlText(bodyNameStream.str());
        bodyNameKey->LinkEndChild(bodyNameTxt);
        (*_blobIt)->LinkEndChild(bodyNameKey);
      }
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionJointFrameReplace(
    std::vector<TiXmlElementPtr>::iterator _blobIt,
    UrdfLinkPtr _link)
{
  std::string linkName = _link->name;
  std::string parentLinkName = _link->getParent()->name;

  if ((*_blobIt)->ValueStr() == "joint")
  {
    // parse it and add/replace the reduction transform
    // find first instance of xyz and rpy, replace with reduction transform
    TiXmlNode* parent = (*_blobIt)->FirstChild("parent");
    if (parent)
    {
      if (GetKeyValueAsString(parent->ToElement()) == linkName)
      {
        (*_blobIt)->RemoveChild(parent);
        TiXmlElement* parentNameKey = new TiXmlElement("parent");
        std::ostringstream parentNameStream;
        parentNameStream << parentLinkName;
        TiXmlText* parentNameTxt = new TiXmlText(parentNameStream.str());
        parentNameKey->LinkEndChild(parentNameTxt);
        (*_blobIt)->LinkEndChild(parentNameKey);
      }
    }
    TiXmlNode* child = (*_blobIt)->FirstChild("child");
    if (child)
    {
      if (GetKeyValueAsString(child->ToElement()) == linkName)
      {
        (*_blobIt)->RemoveChild(child);
        TiXmlElement* childNameKey = new TiXmlElement("child");
        std::ostringstream childNameStream;
        childNameStream << parentLinkName;
        TiXmlText* childNameTxt = new TiXmlText(childNameStream.str());
        childNameKey->LinkEndChild(childNameTxt);
        (*_blobIt)->LinkEndChild(childNameKey);
      }
    }
    /// @todo add anchor offsets if parent link changes location!
  }
}