$search

urdf2gazebo.cpp

Go to the documentation of this file.

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

#include <gazebo/urdf2gazebo.h>
#include "ros/ros.h"
#include "ros/package.h"
#include <fstream>
#include <sstream>

using namespace urdf2gazebo;

URDF2Gazebo::URDF2Gazebo()
{
}

URDF2Gazebo::~URDF2Gazebo()
{
}


std::string URDF2Gazebo::vector32str(const urdf::Vector3 vector, double (*conv)(double) = NULL)
{
    std::stringstream ss;
    ss << (conv ? conv(vector.x) : vector.x);
    ss << " ";
    ss << (conv ? conv(vector.y) : vector.y);
    ss << " ";
    ss << (conv ? conv(vector.z) : vector.z);
    return ss.str();
}

std::string URDF2Gazebo::values2str(unsigned int count, const double *values, double (*conv)(double) = NULL)
{
    std::stringstream ss;
    for (unsigned int i = 0 ; i < count ; i++)
    {
        if (i > 0)
            ss << " ";
        ss << (conv ? conv(values[i]) : values[i]);
    }
    return ss.str();
}

void URDF2Gazebo::setupTransform(btTransform &transform, urdf::Pose pose)
{
    btMatrix3x3 mat;
    mat.setRotation(btQuaternion(pose.rotation.x,pose.rotation.y,pose.rotation.z,pose.rotation.w));
    transform = btTransform(mat,btVector3(pose.position.x,pose.position.y,pose.position.z));
}

void URDF2Gazebo::setupTransform(urdf::Pose &pose, btTransform transform)
{
    btVector3 pz = transform.getOrigin();
    pose.position = urdf::Vector3(pz.x(), pz.y(), pz.z());

    btMatrix3x3 mat = transform.getBasis();
    btQuaternion btq;
    mat.getRotation(btq);
    pose.rotation = urdf::Rotation(btq.x(),btq.y(),btq.z(),btq.w());
}

void URDF2Gazebo::addKeyValue(TiXmlElement *elem, const std::string& key, const std::string &value)
{
    TiXmlElement* child_elem = elem->FirstChildElement(key);
    if (child_elem)
    {
      std::string old_value = getGazeboValue(child_elem);
      if (old_value != value)
        ROS_WARN("multiple inconsistent <%s> exists due to fixed joint reduction, overwriting previous value [%s] with [%s].",key.c_str(),old_value.c_str(),value.c_str());
      else
        ROS_DEBUG("multiple <%s> exists due to fixed joint reduction, overwriting previous value.",key.c_str());
      elem->RemoveChild(child_elem); // remove if one already exists
    }

    TiXmlElement *ekey      = new TiXmlElement(key);
    TiXmlText    *text_ekey = new TiXmlText(value);
    ekey->LinkEndChild(text_ekey);    
    elem->LinkEndChild(ekey); 
}

void URDF2Gazebo::addTransform(TiXmlElement *elem, const::btTransform& transform)
{
    btVector3 pz = transform.getOrigin();
    double cpos[3] = { pz.x(), pz.y(), pz.z() };

    btMatrix3x3 mat = transform.getBasis();
    // mat.getRPY is broken, get back quaternion, use urdf::Rotation to get from quaternion to rpy
    btQuaternion btq;
    mat.getRotation(btq);
    urdf::Rotation q(btq.x(),btq.y(),btq.z(),btq.w());

    double crot[3];
    q.getRPY(crot[0],crot[1],crot[2]);
    //mat.getEulerYPR(crot[2],crot[1],crot[0]); // equivalent to below // bug with gimbal lock
    //mat.getRPY(crot[0],crot[1],crot[2]); // bug with gimbal lock
    
    /* set geometry transform */
    addKeyValue(elem, "xyz", values2str(3, cpos));
    addKeyValue(elem, "rpy", values2str(3, crot, rad2deg));  
}

std::string URDF2Gazebo::getGazeboValue(TiXmlElement* elem)
{
    std::string value_str;
    if (elem->Attribute("value"))
    {
      value_str = elem->Attribute("value");
    }
    else if(elem->FirstChild()) // FIXME: comment out check for now, as ros and system tinyxml does not match.  elem->FirstChild()->Type() == TiXmlNode::TINYXML_TEXT
    {
      value_str = elem->FirstChild()->ValueStr();
    }
    return value_str;
}

void URDF2Gazebo::parseGazeboExtension(TiXmlDocument &urdf_in)
{
  ROS_DEBUG("parsing gazebo extension");
  TiXmlElement* robot_xml = urdf_in.FirstChildElement("robot");

  // Get all Gazebo extension elements, put everything in this->gazebo_extensions_ map, containing a key string (link/joint name) and values
  for (TiXmlElement* gazebo_xml = robot_xml->FirstChildElement("gazebo"); gazebo_xml; gazebo_xml = gazebo_xml->NextSiblingElement("gazebo"))
  {
    const char* ref = gazebo_xml->Attribute("reference");
    std::string ref_str;
    if (!ref)
    {
      ROS_DEBUG("parsing gazebo extension for robot, reference is not specified");
      // copy extensions for robot (outside of link/joint)
      ref_str.clear();
    }
    else
    {
      ROS_DEBUG("parsing gazebo extension for %s",ref);
      // copy extensions for link/joint
      ref_str = std::string(ref);
    }

    if (this->gazebo_extensions_.find(ref_str) == this->gazebo_extensions_.end())
    {
        std::vector<GazeboExtension*> extensions;
        ROS_DEBUG("extension map for reference [%s] does not exist yet, creating new extension map",ref);
        this->gazebo_extensions_.insert(std::make_pair( ref_str, extensions ) );
    }

    // create and insert a new GazeboExtension into the map
    GazeboExtension* gazebo = new GazeboExtension();


    // begin parsing xml node
    for (TiXmlElement *child_elem = gazebo_xml->FirstChildElement(); child_elem; child_elem = child_elem->NextSiblingElement())
    {
      ROS_DEBUG("child element : %s ", child_elem->ValueStr().c_str());

      gazebo->original_reference = ref_str;

      // go through all elements of the extension, extract what we know, and save the rest in blobs

      // material
      if (child_elem->ValueStr() == "material")
      {
          gazebo->material = getGazeboValue(child_elem);
          ROS_DEBUG("   material %s",gazebo->material.c_str());
      }
      else if (child_elem->ValueStr() == "static")
      {
        std::string value_str = getGazeboValue(child_elem);

        // default of setting static flag is false
        if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
          gazebo->setStaticFlag = true;
        else
          gazebo->setStaticFlag = false;

        ROS_DEBUG("   setStaticFlag %d",gazebo->setStaticFlag);

      }
      else if (child_elem->ValueStr() == "turnGravityOff")
      {
        std::string value_str = getGazeboValue(child_elem);

        // default of turnGravityOff is false
        if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
          gazebo->turnGravityOff = true;
        else
          gazebo->turnGravityOff = false;

        ROS_DEBUG("   turnGravityOff %d",gazebo->turnGravityOff);

      }
      else if (child_elem->ValueStr() == "dampingFactor")
      {
          gazebo->is_dampingFactor = true;
          gazebo->dampingFactor = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   dampingFactor %f",gazebo->dampingFactor);
      }
      else if (child_elem->ValueStr() == "maxVel")
      {
          gazebo->is_maxVel = true;
          gazebo->maxVel = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   maxVel %f",gazebo->maxVel);
      }
      else if (child_elem->ValueStr() == "minDepth")
      {
          gazebo->is_minDepth = true;
          gazebo->minDepth = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   minDepth %f",gazebo->minDepth);
      }
      else if (child_elem->ValueStr() == "mu1")
      {
          gazebo->is_mu1 = true;
          gazebo->mu1 = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   mu1 %f",gazebo->mu1);
      }
      else if (child_elem->ValueStr() == "mu2")
      {
          gazebo->is_mu2 = true;
          gazebo->mu2 = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   mu2 %f",gazebo->mu2);
      }
      else if (child_elem->ValueStr() == "fdir1")
      {
          gazebo->fdir1 = getGazeboValue(child_elem);
          ROS_DEBUG("   fdir1 %s",gazebo->fdir1.c_str());
      }
      else if (child_elem->ValueStr() == "kp")
      {
          gazebo->is_kp = true;
          gazebo->kp = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   kp %f",gazebo->kp);
      }
      else if (child_elem->ValueStr() == "kd")
      {
          gazebo->is_kd = true;
          gazebo->kd = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   kd %f",gazebo->kd);
      }
      else if (child_elem->ValueStr() == "genTexCoord")
      {
        std::string value_str = getGazeboValue(child_elem);

        // default of genTexCoord is false
        if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
          gazebo->genTexCoord = true;
        else
          gazebo->genTexCoord = false;

        ROS_DEBUG("   genTexCoord %d",gazebo->genTexCoord);

      }
      else if (child_elem->ValueStr() == "selfCollide")
      {
        std::string value_str = getGazeboValue(child_elem);

        // default of selfCollide is false
        if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
          gazebo->selfCollide = true;
        else
          gazebo->selfCollide = false;

        ROS_DEBUG("   selfCollide %d",gazebo->selfCollide);

      }
      else if (child_elem->ValueStr() == "laserRetro")
      {
          gazebo->is_laserRetro = true;
          gazebo->laserRetro = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   laserRetro %f",gazebo->laserRetro);
      }
      else if (child_elem->ValueStr() == "stopKp")
      {
          gazebo->is_stopKp = true;
          gazebo->stopKp = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   stopKp %f",gazebo->stopKp);
      }
      else if (child_elem->ValueStr() == "stopKd")
      {
          gazebo->is_stopKd = true;
          gazebo->stopKd = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   stopKd %f",gazebo->stopKd);
      }
      else if (child_elem->ValueStr() == "initial_joint_position")
      {
          gazebo->is_initial_joint_position = true;
          gazebo->initial_joint_position = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   initial_joint_position %f",gazebo->initial_joint_position);
      }
      else if (child_elem->ValueStr() == "fudgeFactor")
      {
          gazebo->is_fudgeFactor = true;
          gazebo->fudgeFactor = atof(getGazeboValue(child_elem).c_str());
          ROS_DEBUG("   fudgeFactor %f",gazebo->fudgeFactor);
      }
      else if (child_elem->ValueStr() == "provideFeedback")
      {
          std::string value_str = getGazeboValue(child_elem);

          if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
            gazebo->provideFeedback = true;
          else
            gazebo->provideFeedback = false;

          ROS_DEBUG("   provideFeedback %d",(int)gazebo->provideFeedback);
      }
      else
      {
          std::ostringstream stream;
          stream << *child_elem;
          // save all unknown stuff in a vector of blobs
          TiXmlElement *blob = new TiXmlElement(*child_elem);
          gazebo->blobs.push_back(blob);
          ROS_DEBUG("    blobs %s",stream.str().c_str());
      }
    }

    // insert into my map
    (this->gazebo_extensions_.find(ref_str))->second.push_back(gazebo);
  }
}

void URDF2Gazebo::insertGazeboExtensionGeom(TiXmlElement *elem,std::string link_name)
{
    for (std::map<std::string,std::vector<GazeboExtension*> >::iterator gazebo_it = this->gazebo_extensions_.begin();
         gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
    {
      for (std::vector<GazeboExtension*>::iterator ge = gazebo_it->second.begin(); ge != gazebo_it->second.end(); ge++)
      {
        if ((*ge)->original_reference == link_name)
        {
          // insert mu1, mu2, kp, kd for geom
          if ((*ge)->is_mu1)
              addKeyValue(elem, "mu1", values2str(1, &(*ge)->mu1) );
          if ((*ge)->is_mu2)
              addKeyValue(elem, "mu2", values2str(1, &(*ge)->mu2) );
          if (!(*ge)->fdir1.empty())
              addKeyValue(elem, "fdir1", (*ge)->fdir1);
          if ((*ge)->is_kp)
              addKeyValue(elem, "kp", values2str(1, &(*ge)->kp) );
          if ((*ge)->is_kd)
              addKeyValue(elem, "kd", values2str(1, &(*ge)->kd) );
          if ((*ge)->genTexCoord)
              addKeyValue(elem, "genTexCoord", "true");
          else
              addKeyValue(elem, "genTexCoord", "false");
          if ((*ge)->is_laserRetro)
              addKeyValue(elem, "laserRetro", values2str(1, &(*ge)->laserRetro) );
        }
      }
    }
}

void URDF2Gazebo::insertGazeboExtensionVisual(TiXmlElement *elem,std::string link_name)
{
    for (std::map<std::string,std::vector<GazeboExtension*> >::iterator gazebo_it = this->gazebo_extensions_.begin();
         gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
    {
      for (std::vector<GazeboExtension*>::iterator ge = gazebo_it->second.begin(); ge != gazebo_it->second.end(); ge++)
      {
        if ((*ge)->original_reference == link_name)
        {
          // insert material block
          if (!(*ge)->material.empty())
              addKeyValue(elem, "material", (*ge)->material);
        }
      }
    }
}

void URDF2Gazebo::insertGazeboExtensionBody(TiXmlElement *elem,std::string link_name)
{
    for (std::map<std::string,std::vector<GazeboExtension*> >::iterator gazebo_it = this->gazebo_extensions_.begin();
         gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
    {
      if (gazebo_it->first == link_name)
      {
        ROS_DEBUG("body: reference %s link name %s",gazebo_it->first.c_str(),link_name.c_str());
        for (std::vector<GazeboExtension*>::iterator ge = gazebo_it->second.begin(); ge != gazebo_it->second.end(); ge++)
        {
          // insert turnGravityOff
          if ((*ge)->turnGravityOff)
              addKeyValue(elem, "turnGravityOff", "true");
          else
              addKeyValue(elem, "turnGravityOff", "false");

          // damping factor
          if ((*ge)->is_dampingFactor)
              addKeyValue(elem, "dampingFactor", values2str(1, &(*ge)->dampingFactor) );
          // max contact interpenetration correction velocity
          if ((*ge)->is_maxVel)
              addKeyValue(elem, "maxVel", values2str(1, &(*ge)->maxVel) );
          // contact interpenetration margin tolerance
          if ((*ge)->is_minDepth)
              addKeyValue(elem, "minDepth", values2str(1, &(*ge)->minDepth) );
          // selfCollide tag
          if ((*ge)->selfCollide)
              addKeyValue(elem, "selfCollide", "true");
          else
              addKeyValue(elem, "selfCollide", "false");
          // insert blobs into body
          for (std::vector<TiXmlElement*>::iterator blob_it = (*ge)->blobs.begin();
               blob_it != (*ge)->blobs.end(); blob_it++)
          {
              elem->LinkEndChild((*blob_it)->Clone());
          }
        }
      }
    }
}
void URDF2Gazebo::insertGazeboExtensionJoint(TiXmlElement *elem,std::string joint_name)
{
    for (std::map<std::string,std::vector<GazeboExtension*> >::iterator gazebo_it = this->gazebo_extensions_.begin();
         gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
    {
      if (gazebo_it->first == joint_name)
      {
        for (std::vector<GazeboExtension*>::iterator ge = gazebo_it->second.begin(); ge != gazebo_it->second.end(); ge++)
        {
          ROS_DEBUG("geom: reference %s joint name %s, stopKp %f",gazebo_it->first.c_str(),joint_name.c_str(),(*ge)->stopKp);
          // insert stopKp, stopKd, fudgeFactor
          if ((*ge)->is_stopKp)
              addKeyValue(elem, "stopKp", values2str(1, &(*ge)->stopKp) );
          if ((*ge)->is_stopKd)
              addKeyValue(elem, "stopKd", values2str(1, &(*ge)->stopKd) );
          if ((*ge)->is_initial_joint_position)
              addKeyValue(elem, "initial_joint_position", values2str(1, &(*ge)->initial_joint_position) );
          if ((*ge)->is_fudgeFactor)
              addKeyValue(elem, "fudgeFactor", values2str(1, &(*ge)->fudgeFactor) );

          // insert provideFeedback
          if ((*ge)->provideFeedback)
              addKeyValue(elem, "provideFeedback", "true");
          else
              addKeyValue(elem, "provideFeedback", "false");
        }
      }
    }
}
void URDF2Gazebo::insertGazeboExtensionRobot(TiXmlElement *elem)
{
    for (std::map<std::string,std::vector<GazeboExtension*> >::iterator gazebo_it = this->gazebo_extensions_.begin();
         gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
    {
      if (gazebo_it->first.empty()) // no reference
      {
        for (std::vector<GazeboExtension*>::iterator ge = gazebo_it->second.begin(); ge != gazebo_it->second.end(); ge++)
        {
          // insert static flag
          if ((*ge)->setStaticFlag)
              addKeyValue(elem, "static", "true");
          else
              addKeyValue(elem, "static", "false");
          // insert blobs into robot
          for (std::vector<TiXmlElement*>::iterator blob_it = (*ge)->blobs.begin();
               blob_it != (*ge)->blobs.end(); blob_it++)
          {
              std::ostringstream stream_in;
              stream_in << *(*blob_it);
              ROS_DEBUG("robot: reference empty, blobs for robot\n%s",stream_in.str().c_str());
              elem->LinkEndChild((*blob_it)->Clone());
          }
        }
      }
    }
}

std::string URDF2Gazebo::getGeometrySize(boost::shared_ptr<urdf::Geometry> geometry, int *sizeCount, double *sizeVals)
{
    std::string type("empty");
    
    switch (geometry->type)
    {
    case urdf::Geometry::BOX:
        type = "box";
        *sizeCount = 3;
        {
            boost::shared_ptr<const urdf::Box> box;
            box = boost::dynamic_pointer_cast< const urdf::Box >(geometry);
            sizeVals[0] = box->dim.x;
            sizeVals[1] = box->dim.y;
            sizeVals[2] = box->dim.z;
        }
        break;
    case urdf::Geometry::CYLINDER:
        type = "cylinder";
        *sizeCount = 2;
        {
            boost::shared_ptr<const urdf::Cylinder> cylinder;
            cylinder = boost::dynamic_pointer_cast<const urdf::Cylinder >(geometry);
            sizeVals[0] = cylinder->radius;
            sizeVals[1] = cylinder->length;
        }
        break;
    case urdf::Geometry::SPHERE:
        type = "sphere";
        *sizeCount = 1;
        {
          boost::shared_ptr<const urdf::Sphere> sphere;
          sphere = boost::dynamic_pointer_cast<const urdf::Sphere >(geometry);
          sizeVals[0] = sphere->radius;
        }
        break;
    case urdf::Geometry::MESH:
        type = "trimesh";
        *sizeCount = 3;
        {
          boost::shared_ptr<const urdf::Mesh> mesh;
          mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(geometry);
          sizeVals[0] = mesh->scale.x;
          sizeVals[1] = mesh->scale.y;
          sizeVals[2] = mesh->scale.z;
        }
        break;
    default:
        *sizeCount = 0;
        printf("Unknown body type: %d in geometry\n", geometry->type);
        break;
    }
    
    return type;
}

std::string URDF2Gazebo::getGeometryBoundingBox(boost::shared_ptr<urdf::Geometry> geometry, double *sizeVals)
{
    std::string type;
    
    switch (geometry->type)
    {
    case urdf::Geometry::BOX:
        type = "box";
        {
            boost::shared_ptr<const urdf::Box> box;
            box = boost::dynamic_pointer_cast<const urdf::Box >(geometry);
            sizeVals[0] = box->dim.x;
            sizeVals[1] = box->dim.y;
            sizeVals[2] = box->dim.z;
        }
        break;
    case urdf::Geometry::CYLINDER:
        type = "cylinder";
        {
            boost::shared_ptr<const urdf::Cylinder> cylinder;
            cylinder = boost::dynamic_pointer_cast<const urdf::Cylinder >(geometry);
            sizeVals[0] = cylinder->radius * 2;
            sizeVals[1] = cylinder->radius * 2;
            sizeVals[2] = cylinder->length;
        }
        break;
    case urdf::Geometry::SPHERE:
        type = "sphere";
        {
            boost::shared_ptr<const urdf::Sphere> sphere;
            sphere = boost::dynamic_pointer_cast<const urdf::Sphere >(geometry);
            sizeVals[0] = sizeVals[1] = sizeVals[2] = sphere->radius;
        }
        break;
    case urdf::Geometry::MESH:
        type = "trimesh";
        {
            boost::shared_ptr<const urdf::Mesh> mesh;
            mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(geometry);
            sizeVals[0] = mesh->scale.x;
            sizeVals[1] = mesh->scale.y;
            sizeVals[2] = mesh->scale.z;
        }
        break;
    default:
        sizeVals[0] = sizeVals[1] = sizeVals[2] = 0;
        printf("Unknown body type: %d in geometry\n", geometry->type);
        break;
    }
    
    return type;
}

void URDF2Gazebo::printMass(std::string link_name, dMass mass)
{
      ROS_DEBUG("LINK NAME: [%s] from dMass",link_name.c_str());
      ROS_DEBUG("     MASS: [%f]",mass.mass);
      ROS_DEBUG("       CG: [%f %f %f]",mass.c[0],mass.c[1],mass.c[2]);
      ROS_DEBUG("        I: [%f %f %f]",mass.I[0],mass.I[1],mass.I[2]);
      ROS_DEBUG("           [%f %f %f]",mass.I[4],mass.I[5],mass.I[6]);
      ROS_DEBUG("           [%f %f %f]",mass.I[8],mass.I[9],mass.I[10]);
}

void URDF2Gazebo::printMass(boost::shared_ptr<urdf::Link> link)
{
      ROS_DEBUG("LINK NAME: [%s] from urdf::Link",link->name.c_str());
      ROS_DEBUG("     MASS: [%f]",link->inertial->mass);
      ROS_DEBUG("       CG: [%f %f %f]",link->inertial->origin.position.x,link->inertial->origin.position.y,link->inertial->origin.position.z);
      ROS_DEBUG("        I: [%f %f %f]",link->inertial->ixx,link->inertial->ixy,link->inertial->ixz);
      ROS_DEBUG("           [%f %f %f]",link->inertial->ixy,link->inertial->iyy,link->inertial->iyz);
      ROS_DEBUG("           [%f %f %f]",link->inertial->ixz,link->inertial->iyz,link->inertial->izz);
}


#ifdef ROBOT_MODEL_MULTI_VISUAL_COLLISION
void URDF2Gazebo::reduceFixedJoints(TiXmlElement *root, boost::shared_ptr<urdf::Link> link)
{

  ROS_DEBUG("TREE: at [%s]",link->name.c_str());

  // 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 (link->child_links[i]->parent_joint->type == urdf::Joint::FIXED)
      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 && link->parent_joint->type == urdf::Joint::FIXED)
  {
    ROS_DEBUG("TREE:   extension lumping from [%s] to [%s]",link->name.c_str(),link->getParent()->name.c_str());

    // lump gazebo extensions to parent, (give them new reference link names)
    reduceGazeboExtensionToParent(link);

    ROS_DEBUG("TREE:   mass lumping from [%s] to [%s]",link->name.c_str(),link->getParent()->name.c_str());
    /* now lump all contents of this link to parent */
    if (link->inertial)
    {
      // get parent mass (in parent link frame)
      dMass parent_mass;
      dMassSetParameters(&parent_mass, link->getParent()->inertial->mass,
        link->getParent()->inertial->origin.position.x, link->getParent()->inertial->origin.position.y, link->getParent()->inertial->origin.position.z,
        link->getParent()->inertial->ixx, link->getParent()->inertial->iyy, link->getParent()->inertial->izz,
        link->getParent()->inertial->ixy, link->getParent()->inertial->ixz, link->getParent()->inertial->iyz);
      printMass(link->getParent()->name,parent_mass);
      printMass(link->getParent());
      // set link mass (in link frame)
      dMass link_mass;
      dMassSetParameters(&link_mass, link->inertial->mass,
        link->inertial->origin.position.x, link->inertial->origin.position.y, link->inertial->origin.position.z,
        link->inertial->ixx, link->inertial->iyy, link->inertial->izz,
        link->inertial->ixy, link->inertial->ixz, link->inertial->iyz);
      printMass(link->name,link_mass);
      printMass(link);
      // un-rotate link mass into parent link frame
      dMatrix3 R;
      double phi, theta, psi;
      ROS_DEBUG("debug: %f %f %f",phi,theta,psi);
      link->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(phi,theta,psi);
      dRFromEulerAngles(R, phi, theta, psi);
      dMassRotate(&link_mass, R);
      printMass(link->name,link_mass);
      // un-translate link mass into parent link frame
      dMassTranslate(&link_mass, 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->name,link_mass);
      // now link_mass is in the parent frame, add link_mass to parent_mass
      dMassAdd(&parent_mass,&link_mass); // now parent_mass contains link_mass in parent frame
      printMass(link->getParent()->name,parent_mass);
      // update parent mass
      link->getParent()->inertial->mass = parent_mass.mass;
      link->getParent()->inertial->ixx  = parent_mass.I[0+4*0];
      link->getParent()->inertial->iyy  = parent_mass.I[1+4*1];
      link->getParent()->inertial->izz  = parent_mass.I[2+4*2];
      link->getParent()->inertial->ixy  = parent_mass.I[0+4*1];
      link->getParent()->inertial->ixz  = parent_mass.I[0+4*2];
      link->getParent()->inertial->iyz  = parent_mass.I[1+4*2];
      link->getParent()->inertial->origin.position.x  = parent_mass.c[0];
      link->getParent()->inertial->origin.position.y  = parent_mass.c[1];
      link->getParent()->inertial->origin.position.z  = parent_mass.c[2];
      printMass(link->getParent());
    }

    // lump visual to parent
    // lump all visual to parent, assign group name "lump::"+group name+"::'+link name
    // lump but keep the link name in(/as) the group name, so we can correlate visuals to visuals somehow.
    for (std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<urdf::Visual> > > >::iterator visuals_it = link->visual_groups.begin();
         visuals_it != link->visual_groups.end(); visuals_it++)
    {
      // if it's a "default" mesh, it should be added under "lump:"+link name
      if (visuals_it->first == "default")
      {
        std::string lump_group_name = std::string("lump::")+link->name;
        ROS_DEBUG("adding modified lump group name [%s] to link [%s]",lump_group_name.c_str(),link->getParent()->name.c_str());
        for (std::vector<boost::shared_ptr<urdf::Visual> >::iterator visual_it = visuals_it->second->begin(); visual_it != visuals_it->second->end(); visual_it++)
        {
          // transform visual origin from link frame to parent link frame before adding to parent
          (*visual_it)->origin = transformToParentFrame((*visual_it)->origin, link->parent_joint->parent_to_joint_origin_transform);
          // add the modified visual to parent
          link->getParent()->addVisual(lump_group_name,*visual_it);
        }
      }
      else if (visuals_it->first.find(std::string("lump::")) == 0) // starts with "lump::"
      {
        // if it's a previously lumped mesh, relump under same group_name
        std::string lump_group_name = visuals_it->first;
        ROS_DEBUG("re-lumping group name [%s] to link [%s]",lump_group_name.c_str(),link->getParent()->name.c_str());
        for (std::vector<boost::shared_ptr<urdf::Visual> >::iterator visual_it = visuals_it->second->begin(); visual_it != visuals_it->second->end(); visual_it++)
        {
          // transform visual origin from link frame to parent link frame before adding to parent
          (*visual_it)->origin = transformToParentFrame((*visual_it)->origin, link->parent_joint->parent_to_joint_origin_transform);
          // add the modified visual to parent
          link->getParent()->addVisual(lump_group_name,*visual_it);
        }
      }
    }

    // lump collision parent
    // lump all collision to parent, assign group name "lump::"+group name+"::'+link name
    // lump but keep the link name in(/as) the group name, so we can correlate visuals to collisions somehow.
    for (std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<urdf::Collision> > > >::iterator collisions_it = link->collision_groups.begin();
         collisions_it != link->collision_groups.end(); collisions_it++)
    {
      // if it's a "default" mesh, it should be added under "lump:"+link name
      if (collisions_it->first == "default")
      {
        std::string lump_group_name = std::string("lump::")+link->name;
        ROS_DEBUG("adding modified lump group name [%s] to link [%s]",lump_group_name.c_str(),link->getParent()->name.c_str());
        for (std::vector<boost::shared_ptr<urdf::Collision> >::iterator collision_it = collisions_it->second->begin(); collision_it != collisions_it->second->end(); collision_it++)
        {
          // transform collision origin from link frame to parent link frame before adding to parent
          (*collision_it)->origin = transformToParentFrame((*collision_it)->origin, link->parent_joint->parent_to_joint_origin_transform);
          // add the modified collision to parent
          link->getParent()->addCollision(lump_group_name,*collision_it);
        }
      }
      else if (collisions_it->first.find(std::string("lump::")) == 0) // starts with "lump::"
      {
        // if it's a previously lumped mesh, relump under same group_name
        std::string lump_group_name = collisions_it->first;
        ROS_DEBUG("re-lumping group name [%s] to link [%s]",lump_group_name.c_str(),link->getParent()->name.c_str());
        for (std::vector<boost::shared_ptr<urdf::Collision> >::iterator collision_it = collisions_it->second->begin(); collision_it != collisions_it->second->end(); collision_it++)
        {
          // transform collision origin from link frame to parent link frame before adding to parent
          (*collision_it)->origin = transformToParentFrame((*collision_it)->origin, link->parent_joint->parent_to_joint_origin_transform);
          // add the modified collision to parent
          link->getParent()->addCollision(lump_group_name,*collision_it);
        }
      }
    }
    printCollisionGroups(link->getParent());

    // STRATEGY: later when extracting visuals and collision pairs to construct <geom> for gazebo, given that geom(coillision) wraps visual,
    //           loop through lumped collisions, for each collision,
    //             call createGeom() using visual_groups.find(lump group name) to find potential visuals to create the <geom> tag for the body.
    //           This way, <geom> is created only if collision exists in first place, and if matching visual exists it is used, other a sphere default
    //             is created


    ROS_DEBUG("BEGIN JOINT LUMPING");
    // set child link's parent_joint's parent link to a parent link up stream that does not have a fixed parent_joint
    for (unsigned int i = 0 ; i < link->child_links.size() ; ++i) {
      boost::shared_ptr<urdf::Joint> parent_joint = link->child_links[i]->parent_joint;
      if (parent_joint->type != urdf::Joint::FIXED) {
        // go down the tree until we hit a parent joint that is not fixed
        boost::shared_ptr<urdf::Link> new_parent_link = link;
        btTransform joint_anchor_transform; // set to identity first
        while(new_parent_link->parent_joint && new_parent_link->parent_joint->type == urdf::Joint::FIXED) {
          ROS_DEBUG("  ...JOINT: searching: at [%s] checking if parent [%s] is attachable",new_parent_link->name.c_str(),new_parent_link->getParent()->name.c_str());
          joint_anchor_transform *= joint_anchor_transform;
          parent_joint->parent_to_joint_origin_transform = transformToParentFrame(parent_joint->parent_to_joint_origin_transform,
                                                                 new_parent_link->parent_joint->parent_to_joint_origin_transform);
          new_parent_link = new_parent_link->getParent();
        }
        // now set the link->child_links[i]->parent_joint's parent link to the new_parent_link
        link->child_links[i]->setParent(new_parent_link);
        parent_joint->parent_link_name = new_parent_link->name;
        ROS_DEBUG("  JOINT: reparenting for link %s joint %s from %s to %s",link->child_links[i]->name.c_str(),link->child_links[i]->parent_joint->name.c_str(),link->name.c_str(),new_parent_link->name.c_str());
        // and set the link->child_links[i]->parent_joint's parent_to_joint_orogin_transform as the aggregated
        //   anchor transform?
      }
    }
  }

  // continue down the tree for non-fixed joints
  for (unsigned int i = 0 ; i < link->child_links.size() ; ++i)
    if (link->child_links[i]->parent_joint->type != urdf::Joint::FIXED)
      reduceFixedJoints(root, link->child_links[i]);
}

void URDF2Gazebo::printCollisionGroups(boost::shared_ptr<urdf::Link> link)
{
  ROS_DEBUG("COLLISION LUMPING: link: [%s] contains [%d] collisions",link->name.c_str(),(int)link->collision_groups.size());
  for (std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<urdf::Collision> > > >::iterator cols_it = link->collision_groups.begin();
       cols_it != link->collision_groups.end(); cols_it++)
  {
    ROS_DEBUG("    collision_groups: [%s] has [%d] Collision objects",cols_it->first.c_str(),(int)cols_it->second->size());
    //for (std::vector<boost::shared_ptr<urdf::Collision> >::iterator collision_it = cols_it->second->begin(); collision_it != cols_it->second->end(); collision_it++)
    //  ROS_DEBUG("     origins for debugging: [%s]",...cols_it->first.c_str());
  }
}
#endif

urdf::Pose  URDF2Gazebo::transformToParentFrame(urdf::Pose transform_in_link_frame, urdf::Pose parent_to_link_transform)
{
    urdf::Pose transform_in_parent_link_frame;
#ifdef ROBOT_MODEL_MULTI_VISUAL_COLLISION
    // in the new revision, urdf::Pose has some transform operator overrides
    //   rotate link pose to parent_link frame
    transform_in_parent_link_frame.position = parent_to_link_transform.rotation * transform_in_link_frame.position;
    transform_in_parent_link_frame.rotation = parent_to_link_transform.rotation * transform_in_link_frame.rotation;
    //   translate link to parent_link frame
    transform_in_parent_link_frame.position = parent_to_link_transform.position + transform_in_parent_link_frame.position;
#else
    // copy transforms to btTransform
    btTransform bt_transform_in_link_frame;
    setupTransform(bt_transform_in_link_frame, transform_in_link_frame);
    btTransform bt_parent_to_link_transform;
    setupTransform(bt_parent_to_link_transform, parent_to_link_transform);

    //   rotate link pose to parent_link frame
    btVector3 bt_transform_in_parent_link_frame_pos = bt_parent_to_link_transform.getBasis() * bt_transform_in_link_frame.getOrigin();
    btMatrix3x3 bt_transform_in_parent_link_frame_rot = bt_parent_to_link_transform.getBasis() * bt_transform_in_link_frame.getBasis();
    //   translate link to parent_link frame
    bt_transform_in_parent_link_frame_pos = bt_parent_to_link_transform.getOrigin() + bt_transform_in_parent_link_frame_pos;
    btTransform bt_transform_in_parent_link_frame = btTransform(bt_transform_in_parent_link_frame_rot, bt_transform_in_parent_link_frame_pos);

    // copy transform back to urdf::Pose
    setupTransform(transform_in_parent_link_frame, bt_transform_in_parent_link_frame);
#endif
    return transform_in_parent_link_frame;
}

void URDF2Gazebo::reduceGazeboExtensionToParent(boost::shared_ptr<urdf::Link> link)
{
  std::string link_name = link->name;
  std::string new_link_name = link->getParent()->name;

  ROS_DEBUG("  EXTENSION: Reference lumping from [%s] to [%s]",link_name.c_str(), new_link_name.c_str());

  // update extension map with references to link_name
  listGazeboExtensions();

  std::map<std::string,std::vector<GazeboExtension*> >::iterator ext = this->gazebo_extensions_.find(link_name);
  if (ext != this->gazebo_extensions_.end())
  {

    // update reduction transform (for rays, cameras for now).  FIXME: contact frames too?
    for (std::vector<GazeboExtension*>::iterator ge = ext->second.begin(); ge != ext->second.end(); ge++)
      (*ge)->reduction_transform = transformToParentFrame((*ge)->reduction_transform, link->parent_joint->parent_to_joint_origin_transform);

    // FIXME: if the sensor block has sensor:ray or sensor:camera or sensor:stereo_camera, look for/replace/insert reduction transform into xml block
    for (std::vector<GazeboExtension*>::iterator ge = ext->second.begin(); ge != ext->second.end(); ge++)
      updateGazeboExtensionBlobsReductionTransform((*ge));

    // replace all instances of the link_name string in blobs with new_link_name
    for (std::vector<GazeboExtension*>::iterator ge = ext->second.begin(); ge != ext->second.end(); ge++)
      updateGazeboExtensionFrameReplace((*ge)->blobs, link_name, new_link_name);


    // find existing extension with the new link reference
    std::map<std::string,std::vector<GazeboExtension*> >::iterator new_ext = this->gazebo_extensions_.find(new_link_name);
    // create new_extension if none exists
    if (new_ext == this->gazebo_extensions_.end())
    {
      std::vector<GazeboExtension*> extensions;
      this->gazebo_extensions_.insert(std::make_pair( new_link_name, extensions ) );
      new_ext = this->gazebo_extensions_.find(new_link_name);
    }

    // pop extensions from link's vector and push them into the new link's vector
    for (std::vector<GazeboExtension*>::iterator ge = ext->second.begin(); ge != ext->second.end(); ge++)
    {
      new_ext->second.push_back(*ge);
    }
    ext->second.clear();
  }

  listGazeboExtensions();

  // for empty extensions, search and replace link name patterns with new link name
  for (std::map<std::string,std::vector<GazeboExtension*> >::iterator gazebo_it = this->gazebo_extensions_.begin();
                                                        gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
    if (gazebo_it->first.empty())
      for (std::vector<GazeboExtension*>::iterator ge = gazebo_it->second.begin(); ge != gazebo_it->second.end(); ge++)
        updateGazeboExtensionFrameReplace((*ge)->blobs, link_name, new_link_name);

  listGazeboExtensions();
}

void URDF2Gazebo::updateGazeboExtensionFrameReplace(std::vector<TiXmlElement*> &blobs, std::string link_name, std::string new_link_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 <geom>base_link_geom</geom> and it needs to be reparented to <geom>base_footprint_geom</geom>
  ROS_DEBUG("  STRING REPLACE: instances of link name [%s] with new link name [%s]",link_name.c_str(),new_link_name.c_str());
  for (std::vector<TiXmlElement*>::iterator blob_it = blobs.begin(); blob_it != blobs.end(); blob_it++)
  {


      std::ostringstream stream_in;
      stream_in << *(*blob_it);
      std::string blob = stream_in.str();
      ROS_DEBUG("        INITIAL STRING [%s-->%s]: %s",link_name.c_str(),new_link_name.c_str(),blob.c_str());

      if ((*blob_it)->ValueStr() == "sensor:contact")
      {
        // parse it and add/replace the reduction transform
        // find first instance of xyz and rpy, replace with reduction transform
        TiXmlNode* geomName = (*blob_it)->FirstChild("geom");
        if (geomName)
        {
          ROS_DEBUG("  <geom>%s</geom> : %s --> %s",getGazeboValue(geomName->ToElement()).c_str(),link_name.c_str(),new_link_name.c_str());
          if (getGazeboValue(geomName->ToElement()) == link_name + std::string("_geom"))
          {
            (*blob_it)->RemoveChild(geomName);
            TiXmlElement* geom_name_key = new TiXmlElement("geom");
            std::ostringstream geom_name_stream;
            geom_name_stream << new_link_name << "_geom_" << link_name;
            TiXmlText* geom_name_txt = new TiXmlText(geom_name_stream.str());
            geom_name_key->LinkEndChild(geom_name_txt);
            (*blob_it)->LinkEndChild(geom_name_key);
          }
        }
      }
      if ((*blob_it)->ValueStr() == "controller:gazebo_ros_p3d" ||
          (*blob_it)->ValueStr() == "controller:gazebo_ros_imu" ||
          (*blob_it)->ValueStr() == "controller:gazebo_ros_projector")
      {
        // parse it and add/replace the reduction transform
        // find first instance of xyz and rpy, replace with reduction transform
        TiXmlNode* bodyName = (*blob_it)->FirstChild("bodyName");
        if (bodyName)
        {
          if (getGazeboValue(bodyName->ToElement()) == link_name)
          {
            ROS_DEBUG("  <bodyName>%s</bodyName> : %s --> %s",getGazeboValue(bodyName->ToElement()).c_str(),link_name.c_str(),new_link_name.c_str());
            (*blob_it)->RemoveChild(bodyName);
            TiXmlElement* body_name_key = new TiXmlElement("bodyName");
            std::ostringstream body_name_stream;
            body_name_stream << new_link_name;
            TiXmlText* body_name_txt = new TiXmlText(body_name_stream.str());
            body_name_key->LinkEndChild(body_name_txt);
            (*blob_it)->LinkEndChild(body_name_key);
            ROS_DEBUG("Re-parenting extension [%s] from [%s] to [%s] is broken, needs the transform to be complete",(*blob_it)->ValueStr().c_str(),link_name.c_str(),new_link_name.c_str());
          }
        }
      }
      if ((*blob_it)->ValueStr() == "joint:hinge")
      {
        // parse it and add/replace the reduction transform
        // find first instance of xyz and rpy, replace with reduction transform
        TiXmlNode* body1 = (*blob_it)->FirstChild("body1");
        if (body1)
        {
          if (getGazeboValue(body1->ToElement()) == link_name)
          {
            ROS_DEBUG("  <body1>%s</body1> : %s --> %s",getGazeboValue(body1->ToElement()).c_str(),link_name.c_str(),new_link_name.c_str());
            (*blob_it)->RemoveChild(body1);
            TiXmlElement* body1_name_key = new TiXmlElement("body1");
            std::ostringstream body1_name_stream;
            body1_name_stream << new_link_name;
            TiXmlText* body1_name_txt = new TiXmlText(body1_name_stream.str());
            body1_name_key->LinkEndChild(body1_name_txt);
            (*blob_it)->LinkEndChild(body1_name_key);
          }
        }
        TiXmlNode* body2 = (*blob_it)->FirstChild("body2");
        if (body2)
        {
          if (getGazeboValue(body2->ToElement()) == link_name)
          {
            ROS_DEBUG("  <body2>%s</body2> : %s --> %s",getGazeboValue(body2->ToElement()).c_str(),link_name.c_str(),new_link_name.c_str());
            (*blob_it)->RemoveChild(body2);
            TiXmlElement* body2_name_key = new TiXmlElement("body2");
            std::ostringstream body2_name_stream;
            body2_name_stream << new_link_name;
            TiXmlText* body2_name_txt = new TiXmlText(body2_name_stream.str());
            body2_name_key->LinkEndChild(body2_name_txt);
            (*blob_it)->LinkEndChild(body2_name_key);
          }
        }
        TiXmlNode* anchor = (*blob_it)->FirstChild("anchor");
        if (anchor)
        {
          if (getGazeboValue(anchor->ToElement()) == link_name)
          {
            ROS_DEBUG("  <anchor>%s</anchor> : %s --> %s",getGazeboValue(anchor->ToElement()).c_str(),link_name.c_str(),new_link_name.c_str());
            (*blob_it)->RemoveChild(anchor);
            TiXmlElement* anchor_name_key = new TiXmlElement("anchor");
            std::ostringstream anchor_name_stream;
            anchor_name_stream << new_link_name;
            TiXmlText* anchor_name_txt = new TiXmlText(anchor_name_stream.str());
            anchor_name_key->LinkEndChild(anchor_name_txt);
            (*blob_it)->LinkEndChild(anchor_name_key);
          }
        }
      }


      std::ostringstream stream_out;
      stream_out << *(*blob_it);
      ROS_DEBUG("        STRING REPLACED: %s",stream_out.str().c_str());
  }
}

void URDF2Gazebo::updateGazeboExtensionBlobsReductionTransform(GazeboExtension* ext)
{
  for (std::map<std::string,std::vector<GazeboExtension*> >::iterator gazebo_it = this->gazebo_extensions_.begin(); gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
  {
    if (gazebo_it->second.size() > 0)
    {
      for (std::vector<GazeboExtension*>::iterator ge = gazebo_it->second.begin(); ge != gazebo_it->second.end(); ge++)
      {
        for (std::vector<TiXmlElement*>::iterator blob_it = (*ge)->blobs.begin(); blob_it != (*ge)->blobs.end(); blob_it++)
        {
          // overwrite <xyz> and <rpy> if they exist
          if ((*blob_it)->ValueStr() == "sensor:ray" || (*blob_it)->ValueStr() == "sensor:camera" ||
              (*blob_it)->ValueStr() == "controller:gazebo_ros_p3d" ||
              (*blob_it)->ValueStr() == "controller:gazebo_ros_imu" ||
              (*blob_it)->ValueStr() == "controller:gazebo_ros_projector")
          {
            // parse it and add/replace the reduction transform
            // find first instance of xyz and rpy, replace with reduction transform
            ROS_DEBUG("testing reduction transform for [%s]",(*blob_it)->ValueStr().c_str());
            for (TiXmlNode* el_it = (*blob_it)->FirstChild(); el_it; el_it = el_it->NextSibling())
            {
              std::ostringstream stream_in;
              stream_in << *el_it;
              ROS_DEBUG("    %s",stream_in.str().c_str());
            }

            TiXmlNode* xyz_key = (*blob_it)->FirstChild("xyz");
            if (xyz_key)
              (*blob_it)->RemoveChild(xyz_key);
            TiXmlNode* rpy_key = (*blob_it)->FirstChild("rpy");
            if (rpy_key)
              (*blob_it)->RemoveChild(rpy_key);

            xyz_key = new TiXmlElement("xyz");
            rpy_key = new TiXmlElement("rpy");

            urdf::Vector3 reduction_xyz((*ge)->reduction_transform.position.x,(*ge)->reduction_transform.position.y,(*ge)->reduction_transform.position.z);
            urdf::Rotation reduction_q((*ge)->reduction_transform.rotation.x,(*ge)->reduction_transform.rotation.y,(*ge)->reduction_transform.rotation.z,(*ge)->reduction_transform.rotation.w);

            std::ostringstream xyz_stream, rpy_stream;
            xyz_stream << reduction_xyz.x << " " << reduction_xyz.y << " " << reduction_xyz.z;
            urdf::Vector3 reduction_rpy;
            reduction_q.getRPY(reduction_rpy.x,reduction_rpy.y,reduction_rpy.z); // convert to Euler angles for Gazebo XML
            rpy_stream << reduction_rpy.x*180./M_PI << " " << reduction_rpy.y*180./M_PI << " " << reduction_rpy.z*180./M_PI; // convert to degrees for Gazebo (though ROS is in Radians)

            TiXmlText* xyz_txt = new TiXmlText(xyz_stream.str());
            TiXmlText* rpy_txt = new TiXmlText(rpy_stream.str());

            xyz_key->LinkEndChild(xyz_txt);
            rpy_key->LinkEndChild(rpy_txt);

            (*blob_it)->LinkEndChild(xyz_key);
            (*blob_it)->LinkEndChild(rpy_key);
          }
        }
      }
    }
  }
}

void URDF2Gazebo::listGazeboExtensions()
{
  ROS_DEBUG("====================================================================");
  for (std::map<std::string,std::vector<GazeboExtension*> >::iterator gazebo_it = this->gazebo_extensions_.begin(); gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
  {
    int ext_count = 0;
    for (std::vector<GazeboExtension*>::iterator ge = gazebo_it->second.begin(); ge != gazebo_it->second.end(); ge++)
    {
      if ((*ge)->blobs.size() > 0)
      {
        ROS_DEBUG("  PRINTING [%d] BLOBS for extension [%d] referencing [%s]",(int)(*ge)->blobs.size(),ext_count++,gazebo_it->first.c_str());
        for (std::vector<TiXmlElement*>::iterator blob_it = (*ge)->blobs.begin(); blob_it != (*ge)->blobs.end(); blob_it++)
        {
            std::ostringstream stream_in;
            stream_in << *(*blob_it);
            ROS_DEBUG("    BLOB: %s",stream_in.str().c_str());
        }
      }
    }
  }
  ROS_DEBUG("====================================================================");
}

void URDF2Gazebo::listGazeboExtensions(std::string reference)
{
  ROS_DEBUG("--------------------------------------------------------------------");
  for (std::map<std::string,std::vector<GazeboExtension*> >::iterator gazebo_it = this->gazebo_extensions_.begin(); gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
  {
    if (gazebo_it->first == reference)
    {
      ROS_DEBUG("  PRINTING [%d] EXTENSIONS referencing [%s]",(int)gazebo_it->second.size(),reference.c_str());
      for (std::vector<GazeboExtension*>::iterator ge = gazebo_it->second.begin(); ge != gazebo_it->second.end(); ge++)
      {
        for (std::vector<TiXmlElement*>::iterator blob_it = (*ge)->blobs.begin(); blob_it != (*ge)->blobs.end(); blob_it++)
        {
          std::ostringstream stream_in;
          stream_in << *(*blob_it);
          ROS_DEBUG("    BLOB: %s",stream_in.str().c_str());
        }
      }
    }
  }
  ROS_DEBUG("--------------------------------------------------------------------");
}

void URDF2Gazebo::convertLink(TiXmlElement *root, boost::shared_ptr<const urdf::Link> link, const btTransform &transform, bool enforce_limits,bool reduce_fixed_joints)
{
    btTransform currentTransform = transform;

    // must have an <inertial> block and cannot have zero mass.  allow det(I) == zero, such as in the case of point mass geoms.
    if (link->name != "world" && ((!link->inertial) || (link->inertial->mass == 0.0)))
    {
      if(!link->child_links.empty())
        ROS_WARN("urdf2gazebo: link(%s) has no inertia, %d children link ignored.", link->name.c_str(),(int)link->child_links.size());

      if(!link->child_joints.empty())
        ROS_WARN("urdf2gazebo: link(%s) has no inertia, %d children joint ignored.", link->name.c_str(),(int)link->child_joints.size());

      if(link->parent_joint)
        ROS_WARN("urdf2gazebo: link(%s) has no inertia, parent joint(%s) is ignored.",link->name.c_str(), link->parent_joint->name.c_str());

      ROS_WARN("urdf2gazebo: link(%s) has no inertia, not modeled in gazebo.", link->name.c_str());
      return;
    }

    /* create <body:...> block for non fixed joint attached bodies */
    if ((link->getParent() && link->getParent()->name == "world") || 
        !reduce_fixed_joints || (!link->parent_joint || link->parent_joint->type != urdf::Joint::FIXED))
      createBody(root, link, transform, currentTransform, enforce_limits, reduce_fixed_joints);

    // recurse into children
    for (unsigned int i = 0 ; i < link->child_links.size() ; ++i)
        convertLink(root, link->child_links[i], currentTransform, enforce_limits, reduce_fixed_joints);
}

void URDF2Gazebo::createBody(TiXmlElement *root, boost::shared_ptr<const urdf::Link> link, const btTransform &transform, btTransform &currentTransform, bool enforce_limits, bool reduce_fixed_joints)
{
    int linkGeomSize;
    double linkSize[3];

    /* create new body */
    TiXmlElement *elem     = new TiXmlElement("body:empty"); // FIXME:  does it matter what the collision type of the body is?

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

    /* set mass properties */
    addKeyValue(elem, "massMatrix", "true");
    addKeyValue(elem, "mass", values2str(1, &link->inertial->mass));
    
    addKeyValue(elem, "ixx", values2str(1, &link->inertial->ixx));
    addKeyValue(elem, "ixy", values2str(1, &link->inertial->ixy));
    addKeyValue(elem, "ixz", values2str(1, &link->inertial->ixz));
    addKeyValue(elem, "iyy", values2str(1, &link->inertial->iyy));
    addKeyValue(elem, "iyz", values2str(1, &link->inertial->iyz));
    addKeyValue(elem, "izz", values2str(1, &link->inertial->izz));
    
    addKeyValue(elem, "cx", values2str(1, &link->inertial->origin.position.x));
    addKeyValue(elem, "cy", values2str(1, &link->inertial->origin.position.y));
    addKeyValue(elem, "cz", values2str(1, &link->inertial->origin.position.z));
  
    double roll,pitch,yaw;
    link->inertial->origin.rotation.getRPY(roll,pitch,yaw);
    if (roll != 0 || pitch != 0 || yaw != 0)
        ROS_ERROR("rotation of inertial frame is not supported\n");

    /* compute global transform */
    btTransform localTransform;
    // this is the transform from parent link to current link
    // this transform does not exist for the root link
    if (link->parent_joint)
    {
      ROS_DEBUG("%s has a parent joint",link->name.c_str());
      setupTransform(localTransform, link->parent_joint->parent_to_joint_origin_transform);
      currentTransform *= localTransform;
    }
    else
      ROS_DEBUG("%s has no parent joint",link->name.c_str());


    // create origin tag for this element
    addTransform(elem, currentTransform);

    // make additional geoms using the lumped stuff
    // FIXME:  what's the pairing strategy between visuals and collisions?
    //      when only visuals exists?
    //      when only collisions exists?
    //         originally, there's one visual and one collision per link, which maps nicely to <body><geom><visual> heirarchy
    //         now we have multiple visuals and collisions...  how do we map?
    // loop through all default visuals and lump visuals
#ifdef ROBOT_MODEL_MULTI_VISUAL_COLLISION
    for (std::map<std::string, boost::shared_ptr<std::vector<boost::shared_ptr<urdf::Collision> > > >::const_iterator collisions_it = link->collision_groups.begin();
         collisions_it != link->collision_groups.end(); collisions_it++)
    {
      if (collisions_it->first == "default")
      {
        ROS_DEBUG("creating default geom for link [%s]",link->name.c_str());
        boost::shared_ptr<urdf::Collision> collision = *(collisions_it->second->begin());
        boost::shared_ptr<urdf::Visual> visual = *(link->visual_groups.find(collisions_it->first)->second->begin());

        std::string collision_type = "empty";
        if (!collision || !collision->geometry)
          ROS_DEBUG("urdf2gazebo: link(%s) does not have a collision or geometry tag. No <geometry> will be assigned.", link->name.c_str());
        else
          collision_type = getGeometrySize(collision->geometry, &linkGeomSize, linkSize); // add collision information
        
        /* make a <geom:...> block */
        createGeom(root, elem, link, transform, currentTransform, collision_type, collision, visual, linkGeomSize, linkSize, link->name);
      }
      else if (collisions_it->first.find(std::string("lump::")) == 0) // starts with "lump::"
      {
        ROS_DEBUG("creating lump geom [%s] for link [%s]",collisions_it->first.c_str(),link->name.c_str());
        boost::shared_ptr<urdf::Collision> collision = *(collisions_it->second->begin());
        boost::shared_ptr<urdf::Visual> visual = *(link->visual_groups.find(collisions_it->first)->second->begin());

        std::string collision_type = "empty";
        if (!collision || !collision->geometry)
          ROS_DEBUG("urdf2gazebo: link(%s) does not have a collision or geometry tag. No <geometry> will be assigned.", link->name.c_str());
        else
          collision_type = getGeometrySize(collision->geometry, &linkGeomSize, linkSize); // add collision information
        
        std::string original_reference = collisions_it->first.substr(6);
        /* make a <geom:...> block */
        createGeom(root, elem, link, transform, currentTransform, collision_type, collision, visual, linkGeomSize, linkSize, original_reference);
      }
    }
#else
    {
      ROS_DEBUG("creating default geom for link [%s]",link->name.c_str());
      std::string collision_type = "empty";
      if (!link->collision || !link->collision->geometry)
        ROS_DEBUG("urdf2gazebo: link(%s) does not have a collision or geometry tag. No <geometry> will be assigned.", link->name.c_str());
      else
        collision_type = getGeometrySize(link->collision->geometry, &linkGeomSize, linkSize); // add collision information
      
      /* make a <geom:...> block */
      createGeom(root, elem, link, transform, currentTransform, collision_type, link->collision, link->visual, linkGeomSize, linkSize, link->name);
    }
#endif

    /* copy gazebo extensions data */
    insertGazeboExtensionBody(elem,link->name);
    
    /* add body to document */
    root->LinkEndChild(elem);

    /* make a <joint:...> block */
    createJoint(root, link, currentTransform, enforce_limits, reduce_fixed_joints);

}


void URDF2Gazebo::createJoint(TiXmlElement *root, boost::shared_ptr<const urdf::Link> link, btTransform &currentTransform, bool enforce_limits,bool reduce_fixed_joints)
{
    /* 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 = "hinge";
            break;
        case urdf::Joint::PRISMATIC:
            jtype = "slider";
            break;
        case urdf::Joint::FLOATING:
        case urdf::Joint::PLANAR:
            break;
        case urdf::Joint::FIXED:
            jtype = "fixed";
            break;
        default:
            printf("Unknown joint type: %d in link '%s'\n", link->parent_joint->type, link->name.c_str());
            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" && jtype == "fixed" && reduce_fixed_joints) return;

    if (!jtype.empty())
    {
        TiXmlElement *joint;
        if (jtype == "fixed") joint = new TiXmlElement("joint:hinge");
        else                  joint = new TiXmlElement("joint:" + jtype);
        joint->SetAttribute("name", link->parent_joint->name);
        addKeyValue(joint, "body1", link->name);
        addKeyValue(joint, "body2", link->getParent()->name);
        addKeyValue(joint, "anchor", link->name);
        
        if (jtype == "fixed")
        {
            addKeyValue(joint, "lowStop", "0");
            addKeyValue(joint, "highStop", "0");
            addKeyValue(joint, "axis", "0 0 1");
            addKeyValue(joint, "damping", "0");
        }
        else
        {
            btTransform currentTransformCopy( currentTransform );
            currentTransformCopy.setOrigin( btVector3(0, 0, 0) );
            btVector3 rotatedJointAxis = currentTransformCopy * btVector3( 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(joint, "axis", values2str(3, rotatedJointAxisArray));
            if (link->parent_joint->dynamics)
              addKeyValue(joint, "damping",  values2str(1, &link->parent_joint->dynamics->damping       ));

            // Joint Anchor is deprecated
            // btTransform currentAnchorTransform( currentTransform ); 
            // currentAnchorTransform.setOrigin( btVector3(0, 0, 0) ); 
            // btVector3 jointAnchor = currentAnchorTransform * btVector3( link->parent_joint->anchor.x,
            //                                                             link->parent_joint->anchor.y,
            //                                                             link->parent_joint->anchor.z ); 
            // double tmpAnchor[3] =  { jointAnchor.x(), jointAnchor.y(), jointAnchor.z() };
            // addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
            
            if (enforce_limits && link->parent_joint->limits)
            {
                if (jtype == "slider")
                {
                    addKeyValue(joint, "lowStop",  values2str(1, &link->parent_joint->limits->lower       ));
                    addKeyValue(joint, "highStop", values2str(1, &link->parent_joint->limits->upper       ));
                }
                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)
                    {
                      ROS_WARN("urdf2gazebo: hinge joint limits: lowStop > highStop, setting lowStop to highStop.");
                      *lowstop = *highstop;
                    }
                    addKeyValue(joint, "lowStop",  values2str(1, &link->parent_joint->limits->lower, rad2deg));
                    addKeyValue(joint, "highStop", values2str(1, &link->parent_joint->limits->upper, rad2deg));
                }
            }
        }
        /* copy gazebo extensions data */
        insertGazeboExtensionJoint(joint,link->parent_joint->name);

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


void URDF2Gazebo::createGeom(TiXmlElement *root, TiXmlElement* elem, boost::shared_ptr<const urdf::Link> link, const btTransform &transform, btTransform &currentTransform,std::string collision_type, boost::shared_ptr<urdf::Collision> collision, boost::shared_ptr<urdf::Visual> visual, int linkGeomSize, double* linkSize, std::string original_reference)
{
    /* begin create geometry node, skip if no collision specified */
    if (collision_type != "empty")
    {
        TiXmlElement *geom     = new TiXmlElement("geom:" + collision_type);
        /* set its name */
        if (original_reference == link->name)
          geom->SetAttribute("name", link->name + std::string("_geom"));
        else
          geom->SetAttribute("name", link->name + std::string("_geom_")+original_reference);
        
        /* set transform */
        addKeyValue(geom, "xyz", vector32str(collision->origin.position));
        double rpy[3];
        collision->origin.rotation.getRPY(rpy[0],rpy[1],rpy[2]);
        addKeyValue(geom, "rpy", values2str(3, rpy, rad2deg));
        
        if (collision->geometry->type == urdf::Geometry::MESH)
        {  
            boost::shared_ptr<const urdf::Mesh> mesh;
            mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(collision->geometry);
            /* set mesh size or scale */
            addKeyValue(geom, "scale", vector32str(mesh->scale));

            /* set mesh file */
            /* expand to get absolute mesh filename */
            std::string fullname = mesh->filename;
            if (fullname.find("package://") == 0)
            {
              fullname.erase(0, strlen("package://"));
              size_t pos = fullname.find("/");
              if (pos == std::string::npos)
              {
                ROS_FATAL("Could not parse package:// format for [%s]",mesh->filename.c_str());
              }

              std::string package = fullname.substr(0, pos);
              fullname.erase(0, pos);
              std::string package_path = ros::package::getPath(package);

              if (package_path.empty())
              {
                ROS_FATAL("%s Package[%s] does not exist",mesh->filename.c_str(),package.c_str());
              }

              fullname = package_path + fullname;
            }

            // give some warning if file does not exist.
            std::ifstream fin; fin.open(fullname.c_str(), std::ios::in); fin.close();
            if (fin.fail())
              ROS_ERROR("filename referred by mesh [%s] does not appear to exist.",mesh->filename.c_str());

            // add mesh filename
            addKeyValue(geom, "mesh", fullname);
            
        }
        else
        {
            /* set geometry size */
            addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
        }
        
        /* set additional data from extensions */
        insertGazeboExtensionGeom(geom,original_reference);

        // FIXME: unfortunately, visual is not created if collision is missing!!!
        createVisual(root, geom, link, transform, currentTransform, collision_type, collision, visual,original_reference);

        /* add geometry to body */
        elem->LinkEndChild(geom);
    }
}

void URDF2Gazebo::createVisual(TiXmlElement *root, TiXmlElement *geom, boost::shared_ptr<const urdf::Link> link, const btTransform &transform, btTransform &currentTransform, std::string collision_type, boost::shared_ptr<urdf::Collision> collision, boost::shared_ptr<urdf::Visual> visual,std::string original_reference)
{
    /* begin create gazebo visual node */
    TiXmlElement *gazebo_visual = new TiXmlElement("visual");
    /* compute the visualisation transfrom */
    btTransform coll;

    setupTransform(coll, collision->origin);
    coll.inverse();
    
    btTransform vis;
    if(!visual) 
    {
      ROS_WARN("urdf2gazebo: link(%s) does not have a visual tag, using zero transform.", link->name.c_str());
      setupTransform(vis, urdf::Pose());
    }
    else
      setupTransform(vis, visual->origin);
    coll = coll.inverseTimes(vis);
    
    addTransform(gazebo_visual, coll);
    
    /* set geometry size */                
    
    if (!visual || !visual->geometry)
    {
        ROS_WARN("urdf2gazebo: link(%s) does not have a visual->geometry tag, using default SPHERE with 1mm radius.", link->name.c_str());
        double visualSize[3];
        visualSize[0]=visualSize[1]=visualSize[2]=0.002;
        addKeyValue(gazebo_visual, "scale", values2str(3, visualSize));
        addKeyValue(gazebo_visual, "mesh", "unit_sphere");
    }
    else if (visual->geometry->type == urdf::Geometry::MESH)
    {
        boost::shared_ptr<const urdf::Mesh>  mesh;
        mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(visual->geometry);
        /* set mesh size or scale */
        /*
        if (visual->geometry->isSet["size"])
            addKeyValue(gazebo_visual, "size", values2str(3, mesh->size));        
        else
            addKeyValue(gazebo_visual, "scale", values2str(3, mesh->scale));        
        */
        addKeyValue(gazebo_visual, "scale", vector32str(mesh->scale));

        /* set mesh file */
        if (mesh->filename.empty())
        {
            ROS_ERROR("urdf2gazebo: mesh geometry (%s) was specified but no filename given? using collision type.",link->name.c_str());
            addKeyValue(gazebo_visual, "mesh", "unit_" + collision_type); // reuse collision type if mesh does not specify filename
        }
        else
        {
            /* set mesh file */
            /* expand to get absolute mesh filename */
            std::string fullname = mesh->filename;
            if (fullname.find("package://") == 0)
            {
              fullname.erase(0, strlen("package://"));
              size_t pos = fullname.find("/");
              if (pos == std::string::npos)
              {
                ROS_FATAL("Could not parse package:// format for [%s]",mesh->filename.c_str());
              }

              std::string package = fullname.substr(0, pos);
              fullname.erase(0, pos);
              std::string package_path = ros::package::getPath(package);

              if (package_path.empty())
              {
                ROS_FATAL("%s Package[%s] does not exist",mesh->filename.c_str(),package.c_str());
              }

              fullname = package_path + fullname;
            }

            // add mesh filename
            addKeyValue(gazebo_visual, "mesh", fullname);
        }
        
    }
    else
    {
        double visualSize[3];
        std::string visual_geom_type = getGeometryBoundingBox(visual->geometry, visualSize);
        addKeyValue(gazebo_visual, "scale", values2str(3, visualSize));
        addKeyValue(gazebo_visual, "mesh", "unit_" + visual_geom_type);
    }
    
    /* set additional data from extensions */
    insertGazeboExtensionVisual(gazebo_visual,original_reference);

    /* end create visual node */
    geom->LinkEndChild(gazebo_visual);
}
        

void URDF2Gazebo::walkChildAddNamespace(TiXmlNode* robot_xml,std::string robot_namespace)
{
  TiXmlNode* child = 0;
  child = robot_xml->IterateChildren(child);
  while (child != NULL)
  {
    ROS_DEBUG("recursively walking gazebo extension for %s --> %d",child->ValueStr().c_str(),(int)child->ValueStr().find(std::string("controller")));
    if (child->ValueStr().find(std::string("controller")) == 0 && child->ValueStr().find(std::string("controller")) != std::string::npos)
    {
      if (child->FirstChildElement("robotNamespace") == NULL)
      {
        ROS_DEBUG("    adding robotNamespace for %s",child->ValueStr().c_str());
        addKeyValue(child->ToElement(), "robotNamespace", robot_namespace);
      }
      else
      {
        ROS_DEBUG("    robotNamespace already exists for %s",child->ValueStr().c_str());
      }
    }
    this->walkChildAddNamespace(child,robot_namespace);
    child = robot_xml->IterateChildren(child);
  }
}

void URDF2Gazebo::convert(TiXmlDocument &urdf_in, TiXmlDocument &gazebo_xml_out, bool enforce_limits, urdf::Vector3 initial_xyz, urdf::Vector3 initial_rpy,std::string model_name , std::string robot_namespace, bool xml_declaration)
{

    // copy model to a string
    std::ostringstream stream_in;
    stream_in << urdf_in;

    /* Create a RobotModel from string */
    urdf::Model robot_model;

    if (!robot_model.initString(stream_in.str().c_str()))
    {
        ROS_ERROR("Unable to load robot model from param server robot_description\n");  
        exit(2);
    }


    // add xml declaration if needed
    if (xml_declaration)
    {
      TiXmlDeclaration *decl = new TiXmlDeclaration("1.0", "", "");
      gazebo_xml_out.LinkEndChild(decl);
    }
    
    /* create root element and define needed namespaces */
    TiXmlElement *robot = new TiXmlElement("model:physical");
    robot->SetAttribute("xmlns:gazebo", "http://playerstage.sourceforge.net/gazebo/xmlschema/#gz");
    robot->SetAttribute("xmlns:model", "http://playerstage.sourceforge.net/gazebo/xmlschema/#model");
    robot->SetAttribute("xmlns:sensor", "http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor");
    robot->SetAttribute("xmlns:body", "http://playerstage.sourceforge.net/gazebo/xmlschema/#body");
    robot->SetAttribute("xmlns:geom", "http://playerstage.sourceforge.net/gazebo/xmlschema/#geom");
    robot->SetAttribute("xmlns:joint", "http://playerstage.sourceforge.net/gazebo/xmlschema/#joint");
    robot->SetAttribute("xmlns:controller", "http://playerstage.sourceforge.net/gazebo/xmlschema/#controller");
    robot->SetAttribute("xmlns:interface", "http://playerstage.sourceforge.net/gazebo/xmlschema/#interface");
    robot->SetAttribute("xmlns:rendering", "http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering");
    robot->SetAttribute("xmlns:physics", "http://playerstage.sourceforge.net/gazebo/xmlschema/#physics");
    
    // set model name to urdf robot name if not specified
    if (model_name.empty())
      robot->SetAttribute("name", robot_model.getName());
    else
      robot->SetAttribute("name", model_name);
    
    /* set the transform for the whole model to identity */
    addKeyValue(robot, "xyz", vector32str(initial_xyz));
    addKeyValue(robot, "rpy", vector32str(initial_rpy));
    btTransform transform;
    transform.setIdentity();
    
    /* parse gazebo extension */
    parseGazeboExtension( urdf_in );

    /* start conversion */

    boost::shared_ptr<const urdf::Link> root_link = robot_model.getRoot();
    /* if link connects to parent via fixed joint, lump down and remove link */
    // set reduce_fixed_joints to true will use hinge joints replacements, otherwise, we reduce it down to its parent link recursively
#ifdef ROBOT_MODEL_MULTI_VISUAL_COLLISION
    bool reduce_fixed_joints = true;
    if (reduce_fixed_joints)
      reduceFixedJoints(robot, (boost::const_pointer_cast< urdf::Link >(root_link))); // uncomment to test reduction
#else
    bool reduce_fixed_joints = false;
#endif

    if (root_link->name == "world")
    {
      /* convert all children link */
      for (std::vector<boost::shared_ptr<urdf::Link> >::const_iterator child = root_link->child_links.begin(); child != root_link->child_links.end(); child++)
          convertLink(robot, (*child), transform, enforce_limits, reduce_fixed_joints);
    }
    else
    {
      /* convert, starting from root link */
      convertLink(robot, root_link, transform, enforce_limits, reduce_fixed_joints);
    }
    
    /* find all data item types */
    insertGazeboExtensionRobot(robot);
    
    if (!robot_namespace.empty())
    {
      // traverse all of the: controller::*, add robotNamespace if does not exist already
      ROS_DEBUG("walking gazebo extension for %s",robot->ValueStr().c_str());
      this->walkChildAddNamespace(robot,robot_namespace);
    }

    std::ostringstream stream_robot;
    stream_robot << *(robot);
    ROS_DEBUG("\n\n\nentire robot:\n%s",stream_robot.str().c_str());
    gazebo_xml_out.LinkEndChild(robot);
}

---

gazebo
Author(s): Nate Koenig, Andrew Howard, with contributions from many others. See web page for a full credits llist.
autogenerated on Sat Mar 2 13:39:02 2013