Respect joint axes' initial_position

SDF version 1.6 introduced an initial_position for joint axes.
This is useful for loading models in a collision free pose.
Universal Robot's UR5e is penetrating the ground with the default
initial configuration of all axes set to 0.0.

gazebo/physics/Joint.cc

@@ -384,6 +384,15 @@ void Joint::Init()
       this->SetLowerLimit(0, this->lowerLimit[0]);
       this->SetUpperLimit(0, this->upperLimit[0]);
     }
+
+    if (axisElem->HasElement("initial_position"))
+    {
+      double const initial_position = axisElem->Get<double>("initial_position");
+      if (!this->SetPosition(0, initial_position))
+      {
+        gzerr << "Failed to set initial position " << initial_position << " for joint " << this->GetScopedName() << std::endl;
+      }
+    }
   }
 
   if (this->DOF() >= 2 && this->sdf->HasElement("axis2"))
@@ -401,6 +410,15 @@ void Joint::Init()
       this->SetLowerLimit(1, this->lowerLimit[1]);
       this->SetUpperLimit(1, this->upperLimit[1]);
     }
+
+    if (axisElem->HasElement("initial_position"))
+    {
+      double const initial_position = axisElem->Get<double>("initial_position");
+      if (!this->SetPosition(1, initial_position))
+      {
+        gzerr << "Failed to set initial position " << initial_position << " for joint " << this->GetScopedName() << std::endl;
+      }
+    }
   }
 
   // Set parent name: if parentLink is NULL, it's name be the world