gazebo_tools: urdf2gazebo.cpp Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <gazebo_tools/urdf2gazebo.h>
00036 #include "ros/ros.h"
00037 #include "ros/package.h"
00038 #include <sstream>
00039 
00040 using namespace urdf2gazebo;
00041 
00042 URDF2Gazebo::URDF2Gazebo()
00043 {
00044 }
00045 
00046 URDF2Gazebo::~URDF2Gazebo()
00047 {
00048 }
00049 
00050 
00051 std::string URDF2Gazebo::vector32str(const urdf::Vector3 vector, double (*conv)(double) = NULL)
00052 {
00053     std::stringstream ss;
00054     ss << (conv ? conv(vector.x) : vector.x);
00055     ss << " ";
00056     ss << (conv ? conv(vector.y) : vector.y);
00057     ss << " ";
00058     ss << (conv ? conv(vector.z) : vector.z);
00059     return ss.str();
00060 }
00061 std::string URDF2Gazebo::values2str(unsigned int count, const double *values, double (*conv)(double) = NULL)
00062 {
00063     std::stringstream ss;
00064     for (unsigned int i = 0 ; i < count ; i++)
00065     {
00066         if (i > 0)
00067             ss << " ";
00068         ss << (conv ? conv(values[i]) : values[i]);
00069     }
00070     return ss.str();
00071 }
00072 
00073 void URDF2Gazebo::setupTransform(btTransform &transform, urdf::Pose pose)
00074 {
00075     btMatrix3x3 mat;
00076     mat.setRotation(btQuaternion(pose.rotation.x,pose.rotation.y,pose.rotation.z,pose.rotation.w));
00077     transform = btTransform(mat,btVector3(pose.position.x,pose.position.y,pose.position.z));
00078 }
00079 
00080 void URDF2Gazebo::addKeyValue(TiXmlElement *elem, const std::string& key, const std::string &value)
00081 {
00082     TiXmlElement *ekey      = new TiXmlElement(key);
00083     TiXmlText    *text_ekey = new TiXmlText(value);
00084     ekey->LinkEndChild(text_ekey);    
00085     elem->LinkEndChild(ekey); 
00086 }
00087 
00088 void URDF2Gazebo::addTransform(TiXmlElement *elem, const::btTransform& transform)
00089 {
00090     btVector3 pz = transform.getOrigin();
00091     double cpos[3] = { pz.x(), pz.y(), pz.z() };
00092 
00093     btMatrix3x3 mat = transform.getBasis();
00094     // mat.getRPY is broken, get back quaternion, use urdf::Rotation to get from quaternion to rpy
00095     btQuaternion btq;
00096     mat.getRotation(btq);
00097     urdf::Rotation q(btq.x(),btq.y(),btq.z(),btq.w());
00098 
00099     double crot[3];
00100     q.getRPY(crot[0],crot[1],crot[2]);
00101     //mat.getEulerYPR(crot[2],crot[1],crot[0]); // equivalent to below // bug with gimbal lock
00102     //mat.getRPY(crot[0],crot[1],crot[2]); // bug with gimbal lock
00103     
00104     /* set geometry transform */
00105     addKeyValue(elem, "xyz", values2str(3, cpos));
00106     addKeyValue(elem, "rpy", values2str(3, crot, rad2deg));  
00107 }
00108 
00109 std::string URDF2Gazebo::getGazeboValue(TiXmlElement* elem)
00110 {
00111     std::string value_str;
00112     if (elem->Attribute("value"))
00113     {
00114       value_str = elem->Attribute("value");
00115     }
00116     else if (elem->FirstChild() &&  elem->FirstChild()->Type() == TiXmlNode::TEXT)
00117     {
00118       value_str = elem->FirstChild()->ValueStr();
00119     }
00120     return value_str;
00121 }
00122 
00123 void URDF2Gazebo::parseGazeboExtension(TiXmlDocument &urdf_in)
00124 {
00125   ROS_DEBUG("parsing gazebo extension");
00126   TiXmlElement* robot_xml = urdf_in.FirstChildElement("robot");
00127 
00128   // Get all Gazebo extension elements, put everything in this->gazebo_extensions_ map, containing a key string (link/joint name) and values
00129   for (TiXmlElement* gazebo_xml = robot_xml->FirstChildElement("gazebo"); gazebo_xml; gazebo_xml = gazebo_xml->NextSiblingElement("gazebo"))
00130   {
00131     GazeboExtension *gazebo;
00132 
00133     const char* ref = gazebo_xml->Attribute("reference");
00134     std::string ref_str;
00135     if (!ref)
00136     {
00137       ROS_DEBUG("parsing gazebo extension for robot, reference is not specified");
00138       // copy extensions for robot (outside of link/joint)
00139       ref_str.clear();
00140     }
00141     else
00142     {
00143       ROS_DEBUG("parsing gazebo extension for %s",ref);
00144       // copy extensions for link/joint
00145       ref_str = std::string(ref);
00146     }
00147 
00148     if (gazebo_extensions_.find(ref_str) == gazebo_extensions_.end())
00149     {
00150         gazebo = new GazeboExtension();
00151         ROS_DEBUG("first time parsing, creating extension map %s",ref);
00152     }
00153     else
00154     {
00155         gazebo = (gazebo_extensions_.find(ref_str))->second;
00156         ROS_DEBUG("extension map %s exists, reuse",ref);
00157     }
00158 
00159     // begin parsing xml node
00160     for (TiXmlElement *child_elem = gazebo_xml->FirstChildElement(); child_elem; child_elem = child_elem->NextSiblingElement())
00161     {
00162       ROS_DEBUG("child element : %s ", child_elem->ValueStr().c_str());
00163 
00164       // material
00165       if (child_elem->ValueStr() == "material")
00166       {
00167           gazebo->material = getGazeboValue(child_elem);
00168           ROS_DEBUG("   material %s",gazebo->material.c_str());
00169       }
00170       else if (child_elem->ValueStr() == "static")
00171       {
00172         std::string value_str = getGazeboValue(child_elem);
00173 
00174         // default of setting static flag is false
00175         if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
00176           gazebo->setStaticFlag = true;
00177         else
00178           gazebo->setStaticFlag = false;
00179 
00180         ROS_DEBUG("   setStaticFlag %d",gazebo->setStaticFlag);
00181 
00182       }
00183       else if (child_elem->ValueStr() == "turnGravityOff")
00184       {
00185         std::string value_str = getGazeboValue(child_elem);
00186 
00187         // default of turnGravityOff is false
00188         if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
00189           gazebo->turnGravityOff = true;
00190         else
00191           gazebo->turnGravityOff = false;
00192 
00193         ROS_DEBUG("   turnGravityOff %d",gazebo->turnGravityOff);
00194 
00195       }
00196       else if (child_elem->ValueStr() == "dampingFactor")
00197       {
00198           gazebo->is_dampingFactor = true;
00199           gazebo->dampingFactor = atof(getGazeboValue(child_elem).c_str());
00200           ROS_DEBUG("   dampingFactor %f",gazebo->dampingFactor);
00201       }
00202       else if (child_elem->ValueStr() == "mu1")
00203       {
00204           gazebo->is_mu1 = true;
00205           gazebo->mu1 = atof(getGazeboValue(child_elem).c_str());
00206           ROS_DEBUG("   mu1 %f",gazebo->mu1);
00207       }
00208       else if (child_elem->ValueStr() == "mu2")
00209       {
00210           gazebo->is_mu2 = true;
00211           gazebo->mu2 = atof(getGazeboValue(child_elem).c_str());
00212           ROS_DEBUG("   mu2 %f",gazebo->mu2);
00213       }
00214       else if (child_elem->ValueStr() == "kp")
00215       {
00216           gazebo->is_kp = true;
00217           gazebo->kp = atof(getGazeboValue(child_elem).c_str());
00218           ROS_DEBUG("   kp %f",gazebo->kp);
00219       }
00220       else if (child_elem->ValueStr() == "kd")
00221       {
00222           gazebo->is_kd = true;
00223           gazebo->kd = atof(getGazeboValue(child_elem).c_str());
00224           ROS_DEBUG("   kd %f",gazebo->kd);
00225       }
00226       else if (child_elem->ValueStr() == "genTexCoord")
00227       {
00228         std::string value_str = getGazeboValue(child_elem);
00229 
00230         // default of genTexCoord is false
00231         if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
00232           gazebo->genTexCoord = true;
00233         else
00234           gazebo->genTexCoord = false;
00235 
00236         ROS_DEBUG("   genTexCoord %d",gazebo->genTexCoord);
00237 
00238       }
00239       else if (child_elem->ValueStr() == "selfCollide")
00240       {
00241         std::string value_str = getGazeboValue(child_elem);
00242 
00243         // default of selfCollide is false
00244         if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
00245           gazebo->selfCollide = true;
00246         else
00247           gazebo->selfCollide = false;
00248 
00249         ROS_DEBUG("   selfCollide %d",gazebo->selfCollide);
00250 
00251       }
00252       else if (child_elem->ValueStr() == "laserRetro")
00253       {
00254           gazebo->is_laserRetro = true;
00255           gazebo->laserRetro = atof(getGazeboValue(child_elem).c_str());
00256           ROS_DEBUG("   laserRetro %f",gazebo->laserRetro);
00257       }
00258       else if (child_elem->ValueStr() == "stopKp")
00259       {
00260           gazebo->is_stopKp = true;
00261           gazebo->stopKp = atof(getGazeboValue(child_elem).c_str());
00262           ROS_DEBUG("   stopKp %f",gazebo->stopKp);
00263       }
00264       else if (child_elem->ValueStr() == "stopKd")
00265       {
00266           gazebo->is_stopKd = true;
00267           gazebo->stopKd = atof(getGazeboValue(child_elem).c_str());
00268           ROS_DEBUG("   stopKd %f",gazebo->stopKd);
00269       }
00270       else if (child_elem->ValueStr() == "initial_joint_position")
00271       {
00272           gazebo->is_initial_joint_position = true;
00273           gazebo->initial_joint_position = atof(getGazeboValue(child_elem).c_str());
00274           ROS_DEBUG("   initial_joint_position %f",gazebo->initial_joint_position);
00275       }
00276       else if (child_elem->ValueStr() == "fudgeFactor")
00277       {
00278           gazebo->is_fudgeFactor = true;
00279           gazebo->fudgeFactor = atof(getGazeboValue(child_elem).c_str());
00280           ROS_DEBUG("   fudgeFactor %f",gazebo->fudgeFactor);
00281       }
00282       else if (child_elem->ValueStr() == "provideFeedback")
00283       {
00284           std::string value_str = getGazeboValue(child_elem);
00285 
00286           if (value_str == "true" || value_str == "True" || value_str == "TRUE" || value_str == "yes" || value_str == "1")
00287             gazebo->provideFeedback = true;
00288           else
00289             gazebo->provideFeedback = false;
00290 
00291           ROS_DEBUG("   provideFeedback %d",(int)gazebo->provideFeedback);
00292       }
00293       else
00294       {
00295           std::ostringstream stream;
00296           stream << *child_elem;
00297 
00298           // FIXME: how to copy?
00299           //TiXmlElement *block = new TiXmlElement(stream.str());
00300           TiXmlElement *block = new TiXmlElement(*child_elem);
00301 
00302           gazebo->copy_block.push_back(block);
00303 
00304           ROS_DEBUG("    copy block %s",stream.str().c_str());
00305       }
00306     }
00307 
00308     // insert into my map
00309     this->gazebo_extensions_.insert(std::make_pair( ref_str, gazebo ) );
00310   }
00311 }
00312 
00313 void URDF2Gazebo::insertGazeboExtensionGeom(TiXmlElement *elem,std::string link_name)
00314 {
00315     for (std::map<std::string,GazeboExtension*>::iterator gazebo_it = this->gazebo_extensions_.begin();
00316          gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
00317     {
00318       if (gazebo_it->first == link_name)
00319       {
00320           ROS_DEBUG("geom: reference %s link name %s",gazebo_it->first.c_str(),link_name.c_str());
00321           // insert mu1, mu2, kp, kd for geom
00322           if (gazebo_it->second->is_mu1)
00323               addKeyValue(elem, "mu1", values2str(1, &gazebo_it->second->mu1) );
00324           if (gazebo_it->second->is_mu2)
00325               addKeyValue(elem, "mu2", values2str(1, &gazebo_it->second->mu2) );
00326           if (gazebo_it->second->is_kp)
00327               addKeyValue(elem, "kp", values2str(1, &gazebo_it->second->kp) );
00328           if (gazebo_it->second->is_kd)
00329               addKeyValue(elem, "kd", values2str(1, &gazebo_it->second->kd) );
00330           if (gazebo_it->second->genTexCoord)
00331               addKeyValue(elem, "genTexCoord", "true");
00332           else
00333               addKeyValue(elem, "genTexCoord", "false");
00334           if (gazebo_it->second->is_laserRetro)
00335               addKeyValue(elem, "laserRetro", values2str(1, &gazebo_it->second->laserRetro) );
00336       }
00337     }
00338 }
00339 void URDF2Gazebo::insertGazeboExtensionVisual(TiXmlElement *elem,std::string link_name)
00340 {
00341     for (std::map<std::string,GazeboExtension*>::iterator gazebo_it = this->gazebo_extensions_.begin();
00342          gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
00343     {
00344       if (gazebo_it->first == link_name)
00345       {
00346           ROS_DEBUG("visual: reference %s link name %s, material %s",gazebo_it->first.c_str(),link_name.c_str(),gazebo_it->second->material.c_str());
00347           // insert material block
00348           if (!gazebo_it->second->material.empty())
00349               addKeyValue(elem, "material", gazebo_it->second->material);
00350       }
00351     }
00352 }
00353 void URDF2Gazebo::insertGazeboExtensionBody(TiXmlElement *elem,std::string link_name)
00354 {
00355     for (std::map<std::string,GazeboExtension*>::iterator gazebo_it = this->gazebo_extensions_.begin();
00356          gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
00357     {
00358       if (gazebo_it->first == link_name)
00359       {
00360           ROS_DEBUG("body: reference %s link name %s",gazebo_it->first.c_str(),link_name.c_str());
00361           // insert turnGravityOff
00362           if (gazebo_it->second->turnGravityOff)
00363               addKeyValue(elem, "turnGravityOff", "true");
00364           else
00365               addKeyValue(elem, "turnGravityOff", "false");
00366           // damping factor
00367           if (gazebo_it->second->is_dampingFactor)
00368               addKeyValue(elem, "dampingFactor", values2str(1, &gazebo_it->second->dampingFactor) );
00369           // selfCollide tag
00370           if (gazebo_it->second->selfCollide)
00371               addKeyValue(elem, "selfCollide", "true");
00372           else
00373               addKeyValue(elem, "selfCollide", "false");
00374           // insert copy block into body
00375           for (std::vector<TiXmlElement*>::iterator block_it = gazebo_it->second->copy_block.begin();
00376                block_it != gazebo_it->second->copy_block.end(); block_it++)
00377           {
00378               elem->LinkEndChild((*block_it)->Clone());
00379           }
00380       }
00381     }
00382 }
00383 void URDF2Gazebo::insertGazeboExtensionJoint(TiXmlElement *elem,std::string joint_name)
00384 {
00385     for (std::map<std::string,GazeboExtension*>::iterator gazebo_it = this->gazebo_extensions_.begin();
00386          gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
00387     {
00388       if (gazebo_it->first == joint_name)
00389       {
00390           ROS_DEBUG("geom: reference %s joint name %s, stopKp %f",gazebo_it->first.c_str(),joint_name.c_str(),gazebo_it->second->stopKp);
00391           // insert stopKp, stopKd, fudgeFactor
00392           if (gazebo_it->second->is_stopKp)
00393               addKeyValue(elem, "stopKp", values2str(1, &gazebo_it->second->stopKp) );
00394           if (gazebo_it->second->is_stopKd)
00395               addKeyValue(elem, "stopKd", values2str(1, &gazebo_it->second->stopKd) );
00396           if (gazebo_it->second->is_initial_joint_position)
00397               addKeyValue(elem, "initial_joint_position", values2str(1, &gazebo_it->second->initial_joint_position) );
00398           if (gazebo_it->second->is_fudgeFactor)
00399               addKeyValue(elem, "fudgeFactor", values2str(1, &gazebo_it->second->fudgeFactor) );
00400 
00401           // insert provideFeedback
00402           if (gazebo_it->second->provideFeedback)
00403               addKeyValue(elem, "provideFeedback", "true");
00404           else
00405               addKeyValue(elem, "provideFeedback", "false");
00406       }
00407     }
00408 }
00409 void URDF2Gazebo::insertGazeboExtensionRobot(TiXmlElement *elem)
00410 {
00411     for (std::map<std::string,GazeboExtension*>::iterator gazebo_it = this->gazebo_extensions_.begin();
00412          gazebo_it != this->gazebo_extensions_.end(); gazebo_it++)
00413     {
00414       if (gazebo_it->first.empty()) // no reference
00415       {
00416           // insert static flag
00417           if (gazebo_it->second->setStaticFlag)
00418               addKeyValue(elem, "static", "true");
00419           else
00420               addKeyValue(elem, "static", "false");
00421           // insert copy block into robot
00422           for (std::vector<TiXmlElement*>::iterator block_it = gazebo_it->second->copy_block.begin();
00423                block_it != gazebo_it->second->copy_block.end(); block_it++)
00424           {
00425               std::ostringstream stream_in;
00426               stream_in << *(*block_it);
00427               ROS_DEBUG("robot: reference empty, copy block for robot\n%s",stream_in.str().c_str());
00428               elem->LinkEndChild((*block_it)->Clone());
00429           }
00430       }
00431     }
00432 }
00433 
00434 std::string URDF2Gazebo::getGeometrySize(boost::shared_ptr<urdf::Geometry> geometry, int *sizeCount, double *sizeVals)
00435 {
00436     std::string type;
00437     
00438     switch (geometry->type)
00439     {
00440     case urdf::Geometry::BOX:
00441         type = "box";
00442         *sizeCount = 3;
00443         {
00444             boost::shared_ptr<const urdf::Box> box;
00445             box = boost::dynamic_pointer_cast< const urdf::Box >(geometry);
00446             sizeVals[0] = box->dim.x;
00447             sizeVals[1] = box->dim.y;
00448             sizeVals[2] = box->dim.z;
00449         }
00450         break;
00451     case urdf::Geometry::CYLINDER:
00452         type = "cylinder";
00453         *sizeCount = 2;
00454         {
00455             boost::shared_ptr<const urdf::Cylinder> cylinder;
00456             cylinder = boost::dynamic_pointer_cast<const urdf::Cylinder >(geometry);
00457             sizeVals[0] = cylinder->radius;
00458             sizeVals[1] = cylinder->length;
00459         }
00460         break;
00461     case urdf::Geometry::SPHERE:
00462         type = "sphere";
00463         *sizeCount = 1;
00464         {
00465           boost::shared_ptr<const urdf::Sphere> sphere;
00466           sphere = boost::dynamic_pointer_cast<const urdf::Sphere >(geometry);
00467           sizeVals[0] = sphere->radius;
00468         }
00469         break;
00470     case urdf::Geometry::MESH:
00471         type = "trimesh";
00472         *sizeCount = 3;
00473         {
00474           boost::shared_ptr<const urdf::Mesh> mesh;
00475           mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(geometry);
00476           sizeVals[0] = mesh->scale.x;
00477           sizeVals[1] = mesh->scale.y;
00478           sizeVals[2] = mesh->scale.z;
00479         }
00480         break;
00481     default:
00482         *sizeCount = 0;
00483         printf("Unknown body type: %d in geometry\n", geometry->type);
00484         break;
00485     }
00486     
00487     return type;
00488 }
00489 
00490 std::string URDF2Gazebo::getGeometryBoundingBox(boost::shared_ptr<urdf::Geometry> geometry, double *sizeVals)
00491 {
00492     std::string type;
00493     
00494     switch (geometry->type)
00495     {
00496     case urdf::Geometry::BOX:
00497         type = "box";
00498         {
00499             boost::shared_ptr<const urdf::Box> box;
00500             box = boost::dynamic_pointer_cast<const urdf::Box >(geometry);
00501             sizeVals[0] = box->dim.x;
00502             sizeVals[1] = box->dim.y;
00503             sizeVals[2] = box->dim.z;
00504         }
00505         break;
00506     case urdf::Geometry::CYLINDER:
00507         type = "cylinder";
00508         {
00509             boost::shared_ptr<const urdf::Cylinder> cylinder;
00510             cylinder = boost::dynamic_pointer_cast<const urdf::Cylinder >(geometry);
00511             sizeVals[0] = cylinder->radius * 2;
00512             sizeVals[1] = cylinder->radius * 2;
00513             sizeVals[2] = cylinder->length;
00514         }
00515         break;
00516     case urdf::Geometry::SPHERE:
00517         type = "sphere";
00518         {
00519             boost::shared_ptr<const urdf::Sphere> sphere;
00520             sphere = boost::dynamic_pointer_cast<const urdf::Sphere >(geometry);
00521             sizeVals[0] = sizeVals[1] = sizeVals[2] = sphere->radius;
00522         }
00523         break;
00524     case urdf::Geometry::MESH:
00525         type = "trimesh";
00526         {
00527             boost::shared_ptr<const urdf::Mesh> mesh;
00528             mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(geometry);
00529             sizeVals[0] = mesh->scale.x;
00530             sizeVals[1] = mesh->scale.y;
00531             sizeVals[2] = mesh->scale.z;
00532         }
00533         break;
00534     default:
00535         sizeVals[0] = sizeVals[1] = sizeVals[2] = 0;
00536         printf("Unknown body type: %d in geometry\n", geometry->type);
00537         break;
00538     }
00539     
00540     return type;
00541 }
00542 
00543 void URDF2Gazebo::convertLink(TiXmlElement *root, boost::shared_ptr<const urdf::Link> link, const btTransform &transform, bool enforce_limits)
00544 {
00545     btTransform currentTransform = transform;
00546     std::string collision_type;
00547 
00548     int linkGeomSize;
00549     double linkSize[3];
00550 
00551     if (!link->collision || !link->collision->geometry)
00552     {
00553       ROS_DEBUG("urdf2gazebo: link(%s) does not have a collision or geometry tag. No <geometry> will be assigned.", link->name.c_str());
00554       collision_type = "empty";
00555     }
00556     else
00557       collision_type = getGeometrySize(link->collision->geometry, &linkGeomSize, linkSize); // add collision information
00558     
00559 
00560     if(!link->inertial)
00561     {
00562       ROS_DEBUG("urdf2gazebo: link(%s) without inertial tag and all children joint/links are ignored.", link->name.c_str());
00563       return;
00564     }
00565 
00566     // cannot have zero mass.  allow det(I) == zero, such as in the case of point mass geoms.
00567     if(link->inertial->mass == 0.0)
00568     {
00569       ROS_DEBUG("urdf2gazebo: link(%s) with zero mass/inertia and all children joint/links are ignored.", link->name.c_str());
00570       return;
00571     }
00572 
00573     if (!collision_type.empty())
00574     {
00575         /* create new body */
00576         TiXmlElement *elem     = new TiXmlElement("body:" + collision_type);
00577         
00578         /* set body name */
00579         elem->SetAttribute("name", link->name);
00580 
00581 
00582         /* set mass properties */
00583         addKeyValue(elem, "massMatrix", "true");
00584         addKeyValue(elem, "mass", values2str(1, &link->inertial->mass));
00585         
00586         addKeyValue(elem, "ixx", values2str(1, &link->inertial->ixx));
00587         addKeyValue(elem, "ixy", values2str(1, &link->inertial->ixy));
00588         addKeyValue(elem, "ixz", values2str(1, &link->inertial->ixz));
00589         addKeyValue(elem, "iyy", values2str(1, &link->inertial->iyy));
00590         addKeyValue(elem, "iyz", values2str(1, &link->inertial->iyz));
00591         addKeyValue(elem, "izz", values2str(1, &link->inertial->izz));
00592         
00593         addKeyValue(elem, "cx", values2str(1, &link->inertial->origin.position.x));
00594         addKeyValue(elem, "cy", values2str(1, &link->inertial->origin.position.y));
00595         addKeyValue(elem, "cz", values2str(1, &link->inertial->origin.position.z));
00596         
00597         double roll,pitch,yaw;
00598         link->inertial->origin.rotation.getRPY(roll,pitch,yaw);
00599         if (roll != 0 || pitch != 0 || yaw != 0)
00600             ROS_ERROR("rotation of inertial frame is not supported\n");
00601 
00602         /* compute global transform */
00603         btTransform localTransform;
00604         // this is the transform from parent link to current link
00605         // this transform does not exist for the root link
00606         if (link->parent_joint)
00607         {
00608           ROS_DEBUG("%s has a parent joint",link->name.c_str());
00609           setupTransform(localTransform, link->parent_joint->parent_to_joint_origin_transform);
00610           currentTransform *= localTransform;
00611         }
00612         else
00613           ROS_DEBUG("%s has no parent joint",link->name.c_str());
00614 
00615 
00616 #undef TEST_INIT_JOINT_POSE
00617 #ifdef TEST_INIT_JOINT_POSE
00618         /* compute initial_joint_position transform if specified */
00619         std::map<std::string,GazeboExtension*>::iterator gazebo_initial_joint_position = this->gazebo_extensions_.find(link->parent_joint->name);
00620         if (gazebo_initial_joint_position != this->gazebo_extensions_.end())
00621         {
00622           if (gazebo_initial_joint_position->second->is_initial_joint_position)
00623           {
00624             double initial_position = gazebo_initial_joint_position->second->initial_joint_position;
00625             if (link->parent_joint->type == urdf::Joint::REVOLUTE ||
00626                 link->parent_joint->type == urdf::Joint::CONTINUOUS)
00627             {
00628               ROS_ERROR("setting initial_joint_position for revolute/continuous joint %s",link->parent_joint->name.c_str());
00629 
00630               // convert joint position into a transform based on axis
00631               urdf::Pose initial_pose;
00632               initial_pose.position = urdf::Vector3(0,0,0);
00633               initial_pose.rotation.setFromRPY(link->parent_joint->axis.x*initial_position,
00634                                                link->parent_joint->axis.y*initial_position,
00635                                                link->parent_joint->axis.z*initial_position);
00636 
00637               /* add transform due to intitial joint position */
00638               btTransform initialJointPositionTransform;
00639               // this is the transform due to non-zero initial joint position
00640               setupTransform(initialJointPositionTransform, initial_pose);
00641               // aggregate transform
00642               currentTransform *= initialJointPositionTransform;
00643             }
00644             else if (link->parent_joint->type == urdf::Joint::PRISMATIC)
00645             {
00646               ROS_ERROR("setting initial_joint_position for prismatic joint %s",link->parent_joint->name.c_str());
00647 
00648               // convert joint position into a transform based on axis
00649               urdf::Pose initial_pose;
00650               initial_pose.position = urdf::Vector3(link->parent_joint->axis.x*initial_position,
00651                                                     link->parent_joint->axis.y*initial_position,
00652                                                     link->parent_joint->axis.z*initial_position);
00653               initial_pose.rotation.setFromRPY(0,0,0);
00654 
00655               /* add transform due to intitial joint position */
00656               btTransform initialJointPositionTransform;
00657               // this is the transform due to non-zero initial joint position
00658               setupTransform(initialJointPositionTransform, initial_pose);
00659               // aggregate transform
00660               currentTransform *= initialJointPositionTransform;
00661             }
00662           }
00663         }
00664 #endif
00665 
00666         // create origin tag for this element
00667         addTransform(elem, currentTransform);
00668         
00669         /* begin create geometry node, skip if no collision specified */
00670         if (collision_type != "empty")
00671         {
00672             TiXmlElement *geom     = new TiXmlElement("geom:" + collision_type);
00673             /* set its name */
00674             geom->SetAttribute("name", link->name + "_geom");
00675             
00676             /* set transform */
00677             addKeyValue(geom, "xyz", vector32str(link->collision->origin.position));
00678             double rpy[3];
00679             link->collision->origin.rotation.getRPY(rpy[0],rpy[1],rpy[2]);
00680             addKeyValue(geom, "rpy", values2str(3, rpy, rad2deg));
00681             
00682             if (link->collision->geometry->type == urdf::Geometry::MESH)
00683             {  
00684                 boost::shared_ptr<const urdf::Mesh> mesh;
00685                 mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(link->collision->geometry);
00686                 /* set mesh size or scale */
00687                 addKeyValue(geom, "scale", vector32str(mesh->scale));
00688 
00689                 /* set mesh file */
00690                 /* expand to get absolute mesh filename */
00691                 std::string fullname = mesh->filename;
00692                 if (fullname.find("package://") == 0)
00693                 {
00694                   fullname.erase(0, strlen("package://"));
00695                   size_t pos = fullname.find("/");
00696                   if (pos == std::string::npos)
00697                   {
00698                     ROS_FATAL("Could not parse package:// format for [%s]",mesh->filename.c_str());
00699                   }
00700 
00701                   std::string package = fullname.substr(0, pos);
00702                   fullname.erase(0, pos);
00703                   std::string package_path = ros::package::getPath(package);
00704 
00705                   if (package_path.empty())
00706                   {
00707                     ROS_FATAL("%s Package[%s] does not exist",mesh->filename.c_str(),package.c_str());
00708                   }
00709 
00710                   fullname = package_path + fullname;
00711                 }
00712                 // add mesh filename
00713                 addKeyValue(geom, "mesh", fullname);
00714                 
00715             }
00716             else
00717             {
00718                 /* set geometry size */
00719                 addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
00720             }
00721             
00722             /* set additional data from extensions */
00723             insertGazeboExtensionGeom(geom,link->name);
00724             
00725             /* begin create visual node */
00726             TiXmlElement *visual = new TiXmlElement("visual");
00727             {
00728                 /* compute the visualisation transfrom */
00729                 btTransform coll;
00730                 setupTransform(coll, link->collision->origin);
00731                 coll.inverse();
00732                 
00733                 btTransform vis;
00734                 if(!link->visual) 
00735                 {
00736                   ROS_WARN("urdf2gazebo: link(%s) does not have a visual tag, using zero transform.", link->name.c_str());
00737                   setupTransform(vis, urdf::Pose());
00738                 }
00739                 else
00740                   setupTransform(vis, link->visual->origin);
00741                 coll = coll.inverseTimes(vis);
00742                 
00743                 addTransform(visual, coll);
00744                 
00745                 /* set geometry size */                
00746                 
00747                 if (!link->visual || !link->visual->geometry)
00748                 {
00749                     ROS_WARN("urdf2gazebo: link(%s) does not have a visual->geometry tag, using default SPHERE with 1mm radius.", link->name.c_str());
00750                     double visualSize[3];
00751                     visualSize[0]=visualSize[1]=visualSize[2]=0.002;
00752                     addKeyValue(visual, "scale", values2str(3, visualSize));
00753                     addKeyValue(visual, "mesh", "unit_sphere");
00754                 }
00755                 else if (link->visual->geometry->type == urdf::Geometry::MESH)
00756                 {
00757                     boost::shared_ptr<const urdf::Mesh>  mesh;
00758                     mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(link->visual->geometry);
00759                     /* set mesh size or scale */
00760                     /*
00761                     if (link->visual->geometry->isSet["size"])
00762                         addKeyValue(visual, "size", values2str(3, mesh->size));        
00763                     else
00764                         addKeyValue(visual, "scale", values2str(3, mesh->scale));        
00765                     */
00766                     addKeyValue(visual, "scale", vector32str(mesh->scale));
00767 
00768                     /* set mesh file */
00769                     if (mesh->filename.empty())
00770                     {
00771                         ROS_ERROR("urdf2gazebo: mesh geometry (%s) was specified but no filename given? using collision type.",link->name.c_str());
00772                         addKeyValue(visual, "mesh", "unit_" + collision_type); // reuse collision type if mesh does not specify filename
00773                     }
00774                     else
00775                     {
00776                         /* set mesh file */
00777                         /* expand to get absolute mesh filename */
00778                         std::string fullname = mesh->filename;
00779                         if (fullname.find("package://") == 0)
00780                         {
00781                           fullname.erase(0, strlen("package://"));
00782                           size_t pos = fullname.find("/");
00783                           if (pos == std::string::npos)
00784                           {
00785                             ROS_FATAL("Could not parse package:// format for [%s]",mesh->filename.c_str());
00786                           }
00787 
00788                           std::string package = fullname.substr(0, pos);
00789                           fullname.erase(0, pos);
00790                           std::string package_path = ros::package::getPath(package);
00791 
00792                           if (package_path.empty())
00793                           {
00794                             ROS_FATAL("%s Package[%s] does not exist",mesh->filename.c_str(),package.c_str());
00795                           }
00796 
00797                           fullname = package_path + fullname;
00798                         }
00799 
00800                         // add mesh filename
00801                         addKeyValue(visual, "mesh", fullname);
00802                     }
00803                     
00804                 }
00805                 else
00806                 {
00807                     double visualSize[3];
00808                     std::string visual_geom_type = getGeometryBoundingBox(link->visual->geometry, visualSize);
00809                     addKeyValue(visual, "scale", values2str(3, visualSize));
00810                     addKeyValue(visual, "mesh", "unit_" + visual_geom_type);
00811                 }
00812                 
00813                 /* copy gazebo extensions data */
00814                 insertGazeboExtensionVisual(visual,link->name);
00815 
00816             }
00817             /* end create visual node */
00818             
00819             geom->LinkEndChild(visual);
00820 
00821             /* add geometry to body */
00822             elem->LinkEndChild(geom);      
00823         }
00824         /* end create geometry node */
00825         
00826         
00827         /* copy gazebo extensions data */
00828         insertGazeboExtensionBody(elem,link->name);
00829         
00830         /* add body to document */
00831         root->LinkEndChild(elem);
00832         
00833         /* compute the joint tag */
00834         bool fixed = false;
00835         std::string jtype;
00836         jtype.clear();
00837         if (link->parent_joint != NULL)
00838         {
00839           switch (link->parent_joint->type)
00840           {
00841             case urdf::Joint::CONTINUOUS:
00842             case urdf::Joint::REVOLUTE:
00843                 jtype = "hinge";
00844                 break;
00845             case urdf::Joint::PRISMATIC:
00846                 jtype = "slider";
00847                 break;
00848             case urdf::Joint::FLOATING:
00849             case urdf::Joint::PLANAR:
00850                 break;
00851             case urdf::Joint::FIXED:
00852                 jtype = "hinge";
00853                 fixed = true;
00854                 break;
00855             default:
00856                 printf("Unknown joint type: %d in link '%s'\n", link->parent_joint->type, link->name.c_str());
00857                 break;
00858           }
00859         }
00860         
00861         if (!jtype.empty())
00862         {
00863             TiXmlElement *joint = new TiXmlElement("joint:" + jtype);
00864             joint->SetAttribute("name", link->parent_joint->name);
00865             
00866             addKeyValue(joint, "body1", link->name);
00867             addKeyValue(joint, "body2", link->getParent()->name);
00868             addKeyValue(joint, "anchor", link->name);
00869             
00870             if (fixed)
00871             {
00872                 addKeyValue(joint, "lowStop", "0");
00873                 addKeyValue(joint, "highStop", "0");
00874                 addKeyValue(joint, "axis", "0 0 1");
00875                 addKeyValue(joint, "damping", "0");
00876             }
00877             else
00878             {
00879                 btTransform currentTransformCopy( currentTransform );
00880                 currentTransformCopy.setOrigin( btVector3(0, 0, 0) );
00881                 btVector3 rotatedJointAxis = currentTransformCopy * btVector3( link->parent_joint->axis.x,
00882                                                                                link->parent_joint->axis.y,
00883                                                                                link->parent_joint->axis.z );
00884                 double rotatedJointAxisArray[3] = { rotatedJointAxis.x(), rotatedJointAxis.y(), rotatedJointAxis.z() };
00885                 addKeyValue(joint, "axis", values2str(3, rotatedJointAxisArray));
00886                 if (link->parent_joint->dynamics)
00887                   addKeyValue(joint, "damping",  values2str(1, &link->parent_joint->dynamics->damping       ));
00888 
00889                 // Joint Anchor is deprecated
00890                 // btTransform currentAnchorTransform( currentTransform ); 
00891                 // currentAnchorTransform.setOrigin( btVector3(0, 0, 0) ); 
00892                 // btVector3 jointAnchor = currentAnchorTransform * btVector3( link->parent_joint->anchor.x,
00893                 //                                                             link->parent_joint->anchor.y,
00894                 //                                                             link->parent_joint->anchor.z ); 
00895                 // double tmpAnchor[3] =  { jointAnchor.x(), jointAnchor.y(), jointAnchor.z() };
00896                 // addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
00897                 
00898                 if (enforce_limits && link->parent_joint->limits)
00899                 {
00900                     if (jtype == "slider")
00901                     {
00902                         addKeyValue(joint, "lowStop",  values2str(1, &link->parent_joint->limits->lower       ));
00903                         addKeyValue(joint, "highStop", values2str(1, &link->parent_joint->limits->upper       ));
00904                     }
00905                     else if (link->parent_joint->type != urdf::Joint::CONTINUOUS)
00906                     {
00907                         double *lowstop  = &link->parent_joint->limits->lower;
00908                         double *highstop = &link->parent_joint->limits->upper;
00909                         // enforce ode bounds, this will need to be fixed
00910                         if (*lowstop > *highstop)
00911                         {
00912                           ROS_WARN("urdf2gazebo: hinge joint limits: lowStop > highStop, setting lowStop to highStop.");
00913                           *lowstop = *highstop;
00914                         }
00915                         addKeyValue(joint, "lowStop",  values2str(1, &link->parent_joint->limits->lower, rad2deg));
00916                         addKeyValue(joint, "highStop", values2str(1, &link->parent_joint->limits->upper, rad2deg));
00917                     }
00918                 }
00919             }
00920             /* copy gazebo extensions data */
00921             insertGazeboExtensionJoint(joint,link->parent_joint->name);
00922 
00923             /* add joint to document */
00924             root->LinkEndChild(joint);
00925         }
00926     }
00927     
00928     for (unsigned int i = 0 ; i < link->child_links.size() ; ++i)
00929         convertLink(root, link->child_links[i], currentTransform, enforce_limits);
00930 }
00931 
00932 void URDF2Gazebo::walkChildAddNamespace(TiXmlNode* robot_xml,std::string robot_namespace)
00933 {
00934   TiXmlNode* child = 0;
00935   child = robot_xml->IterateChildren(child);
00936   while (child != NULL)
00937   {
00938     ROS_DEBUG("recursively walking gazebo extension for %s --> %d",child->ValueStr().c_str(),child->ValueStr().find(std::string("controller")));
00939     if (child->ValueStr().find(std::string("controller")) == 0 && child->ValueStr().find(std::string("controller")) != std::string::npos)
00940     {
00941       if (child->FirstChildElement("robotNamespace") == NULL)
00942       {
00943         ROS_DEBUG("    adding robotNamespace for %s",child->ValueStr().c_str());
00944         addKeyValue(child->ToElement(), "robotNamespace", robot_namespace);
00945       }
00946       else
00947       {
00948         ROS_DEBUG("    robotNamespace already exists for %s",child->ValueStr().c_str());
00949       }
00950     }
00951     this->walkChildAddNamespace(child,robot_namespace);
00952     child = robot_xml->IterateChildren(child);
00953   }
00954 }
00955 
00956 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)
00957 {
00958 
00959     // copy model to a string
00960     std::ostringstream stream_in;
00961     stream_in << urdf_in;
00962 
00963     /* Create a RobotModel from string */
00964     urdf::Model robot_model;
00965 
00966     if (!robot_model.initString(stream_in.str().c_str()))
00967     {
00968         ROS_ERROR("Unable to load robot model from param server robot_description\n");  
00969         exit(2);
00970     }
00971 
00972 
00973     // add xml declaration if needed
00974     if (xml_declaration)
00975     {
00976       TiXmlDeclaration *decl = new TiXmlDeclaration("1.0", "", "");
00977       gazebo_xml_out.LinkEndChild(decl);
00978     }
00979     
00980     /* create root element and define needed namespaces */
00981     TiXmlElement *robot = new TiXmlElement("model:physical");
00982     robot->SetAttribute("xmlns:gazebo", "http://playerstage.sourceforge.net/gazebo/xmlschema/#gz");
00983     robot->SetAttribute("xmlns:model", "http://playerstage.sourceforge.net/gazebo/xmlschema/#model");
00984     robot->SetAttribute("xmlns:sensor", "http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor");
00985     robot->SetAttribute("xmlns:body", "http://playerstage.sourceforge.net/gazebo/xmlschema/#body");
00986     robot->SetAttribute("xmlns:geom", "http://playerstage.sourceforge.net/gazebo/xmlschema/#geom");
00987     robot->SetAttribute("xmlns:joint", "http://playerstage.sourceforge.net/gazebo/xmlschema/#joint");
00988     robot->SetAttribute("xmlns:controller", "http://playerstage.sourceforge.net/gazebo/xmlschema/#controller");
00989     robot->SetAttribute("xmlns:interface", "http://playerstage.sourceforge.net/gazebo/xmlschema/#interface");
00990     robot->SetAttribute("xmlns:rendering", "http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering");
00991     robot->SetAttribute("xmlns:physics", "http://playerstage.sourceforge.net/gazebo/xmlschema/#physics");
00992     
00993     // set model name to urdf robot name if not specified
00994     if (model_name.empty())
00995       robot->SetAttribute("name", robot_model.getName());
00996     else
00997       robot->SetAttribute("name", model_name);
00998     
00999     /* set the transform for the whole model to identity */
01000     addKeyValue(robot, "xyz", vector32str(initial_xyz));
01001     addKeyValue(robot, "rpy", vector32str(initial_rpy));
01002     btTransform transform;
01003     transform.setIdentity();
01004     
01005     /* parse gazebo extension */
01006     parseGazeboExtension( urdf_in );
01007 
01008     /* start conversion */
01009     boost::shared_ptr<const urdf::Link> root_link = robot_model.getRoot();
01010 
01011     if (root_link->name == "world")
01012     {
01013       /* convert all children link */
01014       for (std::vector<boost::shared_ptr<urdf::Link> >::const_iterator child = root_link->child_links.begin(); child != root_link->child_links.end(); child++)
01015           convertLink(robot, (*child), transform, enforce_limits);
01016     }
01017     else
01018     {
01019       /* convert, starting from root link */
01020       convertLink(robot, root_link, transform, enforce_limits);
01021     }
01022     
01023     /* find all data item types */
01024     insertGazeboExtensionRobot(robot);
01025     
01026     if (!robot_namespace.empty())
01027     {
01028       // traverse all of the: controller::*, add robotNamespace if does not exist already
01029       ROS_DEBUG("walking gazebo extension for %s",robot->ValueStr().c_str());
01030       this->walkChildAddNamespace(robot,robot_namespace);
01031     }
01032 
01033     std::ostringstream stream_robot;
01034     stream_robot << *(robot);
01035     ROS_DEBUG("\n\n\nentire robot:\n%s",stream_robot.str().c_str());
01036     gazebo_xml_out.LinkEndChild(robot);
01037 }
01038 
01039 
01040 
01041 
01042 
01043 
01044 
01045 
01046 
01047