Index: sdformat-4.1.1/src/parser_urdf.cc
===================================================================
--- sdformat-4.1.1.orig/src/parser_urdf.cc
+++ sdformat-4.1.1/src/parser_urdf.cc
@@ -25,6 +25,7 @@
 #include "urdf_model/model.h"
 #include "urdf_model/link.h"
 #include "urdf_parser/urdf_parser.h"
+#include <urdf_model/utils.h>
 
 #include "sdf/SDFExtension.hh"
 #include "sdf/parser_urdf.hh"
@@ -32,10 +33,10 @@
 
 using namespace sdf;
 
-typedef boost::shared_ptr<urdf::Collision> UrdfCollisionPtr;
-typedef boost::shared_ptr<urdf::Visual> UrdfVisualPtr;
-typedef boost::shared_ptr<urdf::Link> UrdfLinkPtr;
-typedef boost::shared_ptr<const urdf::Link> ConstUrdfLinkPtr;
+typedef std::shared_ptr<urdf::Collision> UrdfCollisionPtr;
+typedef std::shared_ptr<urdf::Visual> UrdfVisualPtr;
+typedef std::shared_ptr<urdf::Link> UrdfLinkPtr;
+typedef std::shared_ptr<const urdf::Link> ConstUrdfLinkPtr;
 typedef std::shared_ptr<TiXmlElement> TiXmlElementPtr;
 typedef std::shared_ptr<SDFExtension> SDFExtensionPtr;
 typedef std::map<std::string, std::vector<SDFExtensionPtr> >
@@ -78,7 +79,7 @@ void InsertSDFExtensionJoint(TiXmlElemen
 /// reduced fixed joints:  check if a fixed joint should be lumped
 ///   checking both the joint type and if disabledFixedJointLumping
 ///   option is set
-bool FixedJointShouldBeReduced(boost::shared_ptr<urdf::Joint> _jnt);
+bool FixedJointShouldBeReduced(std::shared_ptr<urdf::Joint> _jnt);
 
 /// reduced fixed joints:  apply transform reduction for ray sensors
 ///   in extensions when doing fixed joint reduction
@@ -217,9 +218,9 @@ std::string Values2str(unsigned int _cou
 
 
 void CreateGeometry(TiXmlElement* _elem,
-    boost::shared_ptr<urdf::Geometry> _geometry);
+    std::shared_ptr<urdf::Geometry> _geometry);
 
-std::string GetGeometryBoundingBox(boost::shared_ptr<urdf::Geometry> _geometry,
+std::string GetGeometryBoundingBox(std::shared_ptr<urdf::Geometry> _geometry,
     double *_sizeVals);
 
 ignition::math::Pose3d inverseTransformToParentFrame(
@@ -254,7 +255,7 @@ urdf::Vector3 ParseVector3(const std::st
   std::vector<std::string> pieces;
   std::vector<double> vals;
 
-  boost::split(pieces, _str, boost::is_any_of(" "));
+  urdf::split_string(pieces, _str, " ");
   for (unsigned int i = 0; i < pieces.size(); ++i)
   {
     if (pieces[i] != "")
@@ -262,7 +263,7 @@ urdf::Vector3 ParseVector3(const std::st
       try
       {
         vals.push_back(_scale
-            * boost::lexical_cast<double>(pieces[i].c_str()));
+            * std::stod(pieces[i].c_str()));
       }
       catch(boost::bad_lexical_cast &)
       {
@@ -349,7 +350,7 @@ void ReduceCollisionToParent(UrdfLinkPtr
     UrdfCollisionPtr _collision)
 {
 #ifndef URDF_GE_0P3
-  boost::shared_ptr<std::vector<UrdfCollisionPtr> > cols;
+  std::shared_ptr<std::vector<UrdfCollisionPtr> > cols;
   cols = _parentLink->getCollisions(_name);
 
   if (!cols)
@@ -427,7 +428,7 @@ void ReduceVisualToParent(UrdfLinkPtr _p
     UrdfVisualPtr _visual)
 {
 #ifndef URDF_GE_0P3
-  boost::shared_ptr<std::vector<UrdfVisualPtr> > viss;
+  std::shared_ptr<std::vector<UrdfVisualPtr> > viss;
   viss = _parentLink->getVisuals(_name);
 
   if (!viss)
@@ -950,7 +951,7 @@ void ReduceVisualsToParent(UrdfLinkPtr _
   // (original parent link name before lumping/reducing).
 #ifndef URDF_GE_0P3
   for (std::map<std::string,
-      boost::shared_ptr<std::vector<UrdfVisualPtr> > >::iterator
+      std::shared_ptr<std::vector<UrdfVisualPtr> > >::iterator
       visualsIt = _link->visual_groups.begin();
       visualsIt != _link->visual_groups.end(); ++visualsIt)
   {
@@ -1057,7 +1058,7 @@ void ReduceCollisionsToParent(UrdfLinkPt
   // (original parent link name before lumping/reducing).
 #ifndef URDF_GE_0P3
   for (std::map<std::string,
-      boost::shared_ptr<std::vector<UrdfCollisionPtr> > >::iterator
+      std::shared_ptr<std::vector<UrdfCollisionPtr> > >::iterator
       collisionsIt = _link->collision_groups.begin();
       collisionsIt != _link->collision_groups.end(); ++collisionsIt)
   {
@@ -1160,7 +1161,7 @@ void ReduceJointsToParent(UrdfLinkPtr _l
   // a parent link up stream that does not have a fixed parentJoint
   for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i)
   {
-    boost::shared_ptr<urdf::Joint> parentJoint =
+    std::shared_ptr<urdf::Joint> parentJoint =
       _link->child_links[i]->parent_joint;
     if (!FixedJointShouldBeReduced(parentJoint))
     {
@@ -1431,31 +1432,31 @@ void URDF2SDF::ParseSDFExtension(TiXmlDo
       else if (childElem->ValueStr() == "dampingFactor")
       {
         sdf->isDampingFactor = true;
-        sdf->dampingFactor = boost::lexical_cast<double>(
+        sdf->dampingFactor = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "maxVel")
       {
         sdf->isMaxVel = true;
-        sdf->maxVel = boost::lexical_cast<double>(
+        sdf->maxVel = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "minDepth")
       {
         sdf->isMinDepth = true;
-        sdf->minDepth = boost::lexical_cast<double>(
+        sdf->minDepth = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "mu1")
       {
         sdf->isMu1 = true;
-        sdf->mu1 = boost::lexical_cast<double>(
+        sdf->mu1 = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "mu2")
       {
         sdf->isMu2 = true;
-        sdf->mu2 = boost::lexical_cast<double>(
+        sdf->mu2 = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "fdir1")
@@ -1465,13 +1466,13 @@ void URDF2SDF::ParseSDFExtension(TiXmlDo
       else if (childElem->ValueStr() == "kp")
       {
         sdf->isKp = true;
-        sdf->kp = boost::lexical_cast<double>(
+        sdf->kp = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "kd")
       {
         sdf->isKd = true;
-        sdf->kd = boost::lexical_cast<double>(
+        sdf->kd = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "selfCollide")
@@ -1488,13 +1489,13 @@ void URDF2SDF::ParseSDFExtension(TiXmlDo
       else if (childElem->ValueStr() == "maxContacts")
       {
         sdf->isMaxContacts = true;
-        sdf->maxContacts = boost::lexical_cast<int>(
+        sdf->maxContacts = std::stoi(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "laserRetro")
       {
         sdf->isLaserRetro = true;
-        sdf->laserRetro = boost::lexical_cast<double>(
+        sdf->laserRetro = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "springReference")
@@ -1510,37 +1511,37 @@ void URDF2SDF::ParseSDFExtension(TiXmlDo
       else if (childElem->ValueStr() == "stopCfm")
       {
         sdf->isStopCfm = true;
-        sdf->stopCfm = boost::lexical_cast<double>(
+        sdf->stopCfm = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "stopErp")
       {
         sdf->isStopErp = true;
-        sdf->stopErp = boost::lexical_cast<double>(
+        sdf->stopErp = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "stopKp")
       {
         sdf->isStopKp = true;
-        sdf->stopKp = boost::lexical_cast<double>(
+        sdf->stopKp = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "stopKd")
       {
         sdf->isStopKd = true;
-        sdf->stopKd = boost::lexical_cast<double>(
+        sdf->stopKd = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "initialJointPosition")
       {
         sdf->isInitialJointPosition = true;
-        sdf->initialJointPosition = boost::lexical_cast<double>(
+        sdf->initialJointPosition = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "fudgeFactor")
       {
         sdf->isFudgeFactor = true;
-        sdf->fudgeFactor = boost::lexical_cast<double>(
+        sdf->fudgeFactor = std::stod(
             GetKeyValueAsString(childElem).c_str());
       }
       else if (childElem->ValueStr() == "provideFeedback")
@@ -1917,7 +1918,7 @@ void InsertSDFExtensionCollision(TiXmlEl
           if ((*ge)->isMaxContacts)
           {
             AddKeyValue(_elem, "max_contacts",
-                boost::lexical_cast<std::string>((*ge)->maxContacts));
+                std::to_string((*ge)->maxContacts));
           }
         }
       }
@@ -2339,7 +2340,7 @@ void InsertSDFExtensionRobot(TiXmlElemen
 
 ////////////////////////////////////////////////////////////////////////////////
 void CreateGeometry(TiXmlElement* _elem,
-    boost::shared_ptr<urdf::Geometry> _geom)
+    std::shared_ptr<urdf::Geometry> _geom)
 {
   TiXmlElement *sdfGeometry = new TiXmlElement("geometry");
 
@@ -2351,8 +2352,8 @@ void CreateGeometry(TiXmlElement* _elem,
     case urdf::Geometry::BOX:
       type = "box";
       {
-        boost::shared_ptr<const urdf::Box> box;
-        box = boost::dynamic_pointer_cast< const urdf::Box >(_geom);
+        std::shared_ptr<const urdf::Box> box;
+        box = std::dynamic_pointer_cast< const urdf::Box >(_geom);
         int sizeCount = 3;
         double sizeVals[3];
         sizeVals[0] = box->dim.x;
@@ -2366,8 +2367,8 @@ void CreateGeometry(TiXmlElement* _elem,
     case urdf::Geometry::CYLINDER:
       type = "cylinder";
       {
-        boost::shared_ptr<const urdf::Cylinder> cylinder;
-        cylinder = boost::dynamic_pointer_cast<const urdf::Cylinder >(_geom);
+        std::shared_ptr<const urdf::Cylinder> cylinder;
+        cylinder = std::dynamic_pointer_cast<const urdf::Cylinder >(_geom);
         geometryType = new TiXmlElement(type);
         AddKeyValue(geometryType, "length",
             Values2str(1, &cylinder->length));
@@ -2378,8 +2379,8 @@ void CreateGeometry(TiXmlElement* _elem,
     case urdf::Geometry::SPHERE:
       type = "sphere";
       {
-        boost::shared_ptr<const urdf::Sphere> sphere;
-        sphere = boost::dynamic_pointer_cast<const urdf::Sphere >(_geom);
+        std::shared_ptr<const urdf::Sphere> sphere;
+        sphere = std::dynamic_pointer_cast<const urdf::Sphere >(_geom);
         geometryType = new TiXmlElement(type);
         AddKeyValue(geometryType, "radius",
             Values2str(1, &sphere->radius));
@@ -2388,8 +2389,8 @@ void CreateGeometry(TiXmlElement* _elem,
     case urdf::Geometry::MESH:
       type = "mesh";
       {
-        boost::shared_ptr<const urdf::Mesh> mesh;
-        mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(_geom);
+        std::shared_ptr<const urdf::Mesh> mesh;
+        mesh = std::dynamic_pointer_cast<const urdf::Mesh >(_geom);
         geometryType = new TiXmlElement(type);
         AddKeyValue(geometryType, "scale", Vector32Str(mesh->scale));
         // do something more to meshes
@@ -2451,7 +2452,7 @@ void CreateGeometry(TiXmlElement* _elem,
 
 ////////////////////////////////////////////////////////////////////////////////
 std::string GetGeometryBoundingBox(
-    boost::shared_ptr<urdf::Geometry> _geom, double *_sizeVals)
+    std::shared_ptr<urdf::Geometry> _geom, double *_sizeVals)
 {
   std::string type;
 
@@ -2460,8 +2461,8 @@ std::string GetGeometryBoundingBox(
     case urdf::Geometry::BOX:
       type = "box";
       {
-        boost::shared_ptr<const urdf::Box> box;
-        box = boost::dynamic_pointer_cast<const urdf::Box >(_geom);
+        std::shared_ptr<const urdf::Box> box;
+        box = std::dynamic_pointer_cast<const urdf::Box >(_geom);
         _sizeVals[0] = box->dim.x;
         _sizeVals[1] = box->dim.y;
         _sizeVals[2] = box->dim.z;
@@ -2470,8 +2471,8 @@ std::string GetGeometryBoundingBox(
     case urdf::Geometry::CYLINDER:
       type = "cylinder";
       {
-        boost::shared_ptr<const urdf::Cylinder> cylinder;
-        cylinder = boost::dynamic_pointer_cast<const urdf::Cylinder >(_geom);
+        std::shared_ptr<const urdf::Cylinder> cylinder;
+        cylinder = std::dynamic_pointer_cast<const urdf::Cylinder >(_geom);
         _sizeVals[0] = cylinder->radius * 2;
         _sizeVals[1] = cylinder->radius * 2;
         _sizeVals[2] = cylinder->length;
@@ -2480,16 +2481,16 @@ std::string GetGeometryBoundingBox(
     case urdf::Geometry::SPHERE:
       type = "sphere";
       {
-        boost::shared_ptr<const urdf::Sphere> sphere;
-        sphere = boost::dynamic_pointer_cast<const urdf::Sphere >(_geom);
+        std::shared_ptr<const urdf::Sphere> sphere;
+        sphere = std::dynamic_pointer_cast<const urdf::Sphere >(_geom);
         _sizeVals[0] = _sizeVals[1] = _sizeVals[2] = sphere->radius * 2;
       }
       break;
     case urdf::Geometry::MESH:
       type = "trimesh";
       {
-        boost::shared_ptr<const urdf::Mesh> mesh;
-        mesh = boost::dynamic_pointer_cast<const urdf::Mesh >(_geom);
+        std::shared_ptr<const urdf::Mesh> mesh;
+        mesh = std::dynamic_pointer_cast<const urdf::Mesh >(_geom);
         _sizeVals[0] = mesh->scale.x;
         _sizeVals[1] = mesh->scale.y;
         _sizeVals[2] = mesh->scale.z;
@@ -2513,7 +2514,7 @@ void PrintCollisionGroups(UrdfLinkPtr _l
     << static_cast<int>(_link->collision_groups.size())
     << "] collisions.\n";
   for (std::map<std::string,
-      boost::shared_ptr<std::vector<UrdfCollisionPtr > > >::iterator
+      std::shared_ptr<std::vector<UrdfCollisionPtr > > >::iterator
       colsIt = _link->collision_groups.begin();
       colsIt != _link->collision_groups.end(); ++colsIt)
   {
@@ -2906,7 +2907,7 @@ void CreateCollisions(TiXmlElement* _ele
   //   lumped meshes (fixed joint reduction)
 #ifndef URDF_GE_0P3
   for (std::map<std::string,
-      boost::shared_ptr<std::vector<UrdfCollisionPtr> > >::const_iterator
+      std::shared_ptr<std::vector<UrdfCollisionPtr> > >::const_iterator
       collisionsIt = _link->collision_groups.begin();
       collisionsIt != _link->collision_groups.end(); ++collisionsIt)
   {
@@ -3028,7 +3029,7 @@ void CreateVisuals(TiXmlElement* _elem,
   //   lumped meshes (fixed joint reduction)
 #ifndef URDF_GE_0P3
   for (std::map<std::string,
-      boost::shared_ptr<std::vector<UrdfVisualPtr> > >::const_iterator
+      std::shared_ptr<std::vector<UrdfVisualPtr> > >::const_iterator
       visualsIt = _link->visual_groups.begin();
       visualsIt != _link->visual_groups.end(); ++visualsIt)
   {
@@ -3411,7 +3412,7 @@ TiXmlDocument URDF2SDF::InitModelString(
   g_enforceLimits = _enforceLimits;
 
   // Create a RobotModel from string
-  boost::shared_ptr<urdf::ModelInterface> robotModel =
+  std::shared_ptr<urdf::ModelInterface> robotModel =
     urdf::parseURDF(_urdfStr.c_str());
 
   // an xml object to hold the xml result
@@ -3453,7 +3454,7 @@ TiXmlDocument URDF2SDF::InitModelString(
   // fixed joint lumping only for selected joints
   if (g_reduceFixedJoints)
     ReduceFixedJoints(robot,
-        (boost::const_pointer_cast< urdf::Link >(rootLink)));
+        (std::const_pointer_cast< urdf::Link >(rootLink)));
 
   if (rootLink->name == "world")
   {
@@ -3514,7 +3515,7 @@ TiXmlDocument URDF2SDF::InitModelFile(co
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-bool FixedJointShouldBeReduced(boost::shared_ptr<urdf::Joint> _jnt)
+bool FixedJointShouldBeReduced(std::shared_ptr<urdf::Joint> _jnt)
 {
     // A joint should be lumped only if its type is fixed and
     // the disabledFixedJointLumping joint option is not set