From 2b2ec2ff80fcfbfa3d9c61bdbd87c1f0b429d5fe Mon Sep 17 00:00:00 2001
From: "Addisu Z. Taddese" <addisu@openrobotics.org>
Date: Fri, 9 Jul 2021 16:54:27 -0500
Subject: [PATCH] Remove use of deprecated function
 sdf::JointAxis::InitialPosition

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
---
 dartsim/src/SDFFeatures.cc      | 38 ---------------------------------
 dartsim/src/SDFFeatures_TEST.cc |  8 +++----
 dartsim/worlds/test.world       |  4 ----
 3 files changed, 3 insertions(+), 47 deletions(-)

diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
index 8baff95aa..1473d24e6 100644
--- a/dartsim/src/SDFFeatures.cc
+++ b/dartsim/src/SDFFeatures.cc
@@ -126,20 +126,6 @@ static void CopyStandardJointAxisProperties(
     const int _index, Properties &_properties,
     const ::sdf::JointAxis *_sdfAxis)
 {
-#ifndef _WIN32
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-  // TODO(anyone): stop using this deprecated function
-  _properties.mInitialPositions[_index] = _sdfAxis->InitialPosition();
-#ifndef _WIN32
-# pragma GCC diagnostic pop
-#else
-# pragma warning(pop)
-#endif
   _properties.mDampingCoefficients[_index] = _sdfAxis->Damping();
   _properties.mFrictions[_index] = _sdfAxis->Friction();
   _properties.mRestPositions[_index] = _sdfAxis->SpringReference();
@@ -1115,30 +1101,6 @@ Identity SDFFeatures::ConstructSdfJoint(
   joint->setTransformFromParentBodyNode(parent_T_prejoint_init);
   joint->setTransformFromChildBodyNode(child_T_postjoint);
 
-  // This is the transform inside the joint produced by whatever the current
-  // joint position happens to be.
-  const Eigen::Isometry3d T_child_parent_postjoint =
-      _parent ? _child->getTransform(_parent) : _child->getTransform();
-
-  const Eigen::Isometry3d prejoint_T_postjoint =
-      parent_T_prejoint_init.inverse()
-      * T_child_parent_postjoint
-      * child_T_postjoint;
-
-  // This is the corrected transform needed to get the child link to its
-  // correct pose (as specified by the loaded SDF) for the current initial
-  // position
-  const Eigen::Isometry3d T_parent_postjoint =
-      _parent ? _parent->getWorldTransform() : Eigen::Isometry3d::Identity();
-
-  const Eigen::Isometry3d parent_T_prejoint_final =
-      T_parent_postjoint.inverse()
-      * T_child
-      * child_T_postjoint
-      * prejoint_T_postjoint.inverse();
-
-  joint->setTransformFromParentBodyNode(parent_T_prejoint_final);
-
   const std::size_t jointID = this->AddJoint(joint);
 
   return this->GenerateIdentity(jointID, this->joints.at(jointID));
diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc
index 4d25c84fe..d1a1ab42f 100644
--- a/dartsim/src/SDFFeatures_TEST.cc
+++ b/dartsim/src/SDFFeatures_TEST.cc
@@ -323,12 +323,10 @@ TEST_P(SDFFeatures_TEST, CheckDartsimData)
   ASSERT_EQ(3u, skeleton->getNumBodyNodes());
 
   auto verify = [](const dart::dynamics::DegreeOfFreedom * dof,
-                   double initialPos, double damping, double friction,
+                   double damping, double friction,
                    double springRest, double stiffness, double lower,
                    double upper, double maxForce, double maxVelocity)
   {
-    EXPECT_DOUBLE_EQ(initialPos, dof->getPosition());
-    EXPECT_DOUBLE_EQ(initialPos, dof->getInitialPosition());
     EXPECT_DOUBLE_EQ(damping, dof->getDampingCoefficient());
     EXPECT_DOUBLE_EQ(friction, dof->getCoulombFriction());
     EXPECT_DOUBLE_EQ(springRest, dof->getRestPosition());
@@ -344,12 +342,12 @@ TEST_P(SDFFeatures_TEST, CheckDartsimData)
   // Test that things were parsed correctly. These values are either stated or
   // implied in the test.world SDF file.
   verify(skeleton->getJoint(1)->getDof(0),
-         1.5706796, 3.0, 0.0, 0.0, 0.0, -1e16, 1e16,
+         3.0, 0.0, 0.0, 0.0, -1e16, 1e16,
          std::numeric_limits<double>::infinity(),
          std::numeric_limits<double>::infinity());
 
   verify(skeleton->getJoint(2)->getDof(0),
-         -0.429462, 3.0, 0.0, 0.0, 0.0, -1e16, 1e16,
+         3.0, 0.0, 0.0, 0.0, -1e16, 1e16,
          std::numeric_limits<double>::infinity(),
          std::numeric_limits<double>::infinity());
 
diff --git a/dartsim/worlds/test.world b/dartsim/worlds/test.world
index 76eede1a0..c5d9bc604 100644
--- a/dartsim/worlds/test.world
+++ b/dartsim/worlds/test.world
@@ -221,8 +221,6 @@
         <parent>base</parent>
         <child>upper_link</child>
         <axis>
-          <!-- TODO(anyone): stop using deprecated element -->
-          <initial_position>1.5706796</initial_position>
           <xyz>1.0 0 0</xyz>
           <dynamics>
             <damping>3.0</damping>
@@ -234,8 +232,6 @@
         <parent>upper_link</parent>
         <child>lower_link</child>
         <axis>
-          <!-- TODO(anyone): stop using deprecated element -->
-          <initial_position>-0.429462</initial_position>
           <xyz>1.0 0 0</xyz>
           <dynamics>
             <damping>3.0</damping>