Parse URDF continuous joint effort/velocity limits (#684)

* Test URDF continuous joint effort/velocity limits * Fix parsing of URDF continuous joint effort/velocity limits Fixes #683. Signed-off-by: Steve Peters <scpeters@openrobotics.org>
gazebosim · Sep 4, 2021 · f180b39 · f180b39
1 parent 1d77861
commit f180b39
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 8 deletions.
diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
@@ -3016,10 +3016,6 @@ void CreateJoint(TiXmlElement *_root,
                       Values2str(1, &_link->parent_joint->limits->lower));
           AddKeyValue(jointAxisLimit, "upper",
                       Values2str(1, &_link->parent_joint->limits->upper));
-          AddKeyValue(jointAxisLimit, "effort",
-                      Values2str(1, &_link->parent_joint->limits->effort));
-          AddKeyValue(jointAxisLimit, "velocity",
-                      Values2str(1, &_link->parent_joint->limits->velocity));
         }
         else if (_link->parent_joint->type != urdf::Joint::CONTINUOUS)
         {
@@ -3041,11 +3037,11 @@ void CreateJoint(TiXmlElement *_root,
                       Values2str(1, &_link->parent_joint->limits->lower));
           AddKeyValue(jointAxisLimit, "upper",
                       Values2str(1, &_link->parent_joint->limits->upper));
-          AddKeyValue(jointAxisLimit, "effort",
-                      Values2str(1, &_link->parent_joint->limits->effort));
-          AddKeyValue(jointAxisLimit, "velocity",
-                      Values2str(1, &_link->parent_joint->limits->velocity));
         }
+        AddKeyValue(jointAxisLimit, "effort",
+                    Values2str(1, &_link->parent_joint->limits->effort));
+        AddKeyValue(jointAxisLimit, "velocity",
+                    Values2str(1, &_link->parent_joint->limits->velocity));
       }
     }
 

diff --git a/test/integration/urdf_joint_parameters.cc b/test/integration/urdf_joint_parameters.cc
@@ -65,6 +65,13 @@ TEST(SDFParser, JointAxisParameters)
     ASSERT_TRUE(dynamics->HasElement("friction"));
     EXPECT_DOUBLE_EQ(value, dynamics->Get<double>("damping"));
     EXPECT_DOUBLE_EQ(value, dynamics->Get<double>("friction"));
+
+    EXPECT_TRUE(axis->HasElement("limit"));
+    sdf::ElementPtr limit = axis->GetElement("limit");
+    EXPECT_TRUE(limit->HasElement("effort"));
+    EXPECT_TRUE(limit->HasElement("velocity"));
+    EXPECT_DOUBLE_EQ(value, limit->Get<double>("effort"));
+    EXPECT_DOUBLE_EQ(value, limit->Get<double>("velocity"));
   }
   EXPECT_EQ(bitmask, 0x3u);
 

diff --git a/test/integration/urdf_joint_parameters.urdf b/test/integration/urdf_joint_parameters.urdf
@@ -9,6 +9,7 @@
     <parent link="world"/>
     <child link="link0"/>
     <dynamics damping="0.0" friction="0.0" />
+    <limit effort="0.0" velocity="0.0" />
   </joint>
 
   <link name="link0">
@@ -31,6 +32,7 @@
     <parent link="link0"/>
     <child link="link1"/>
     <dynamics damping="1.0" friction="1.0" />
+    <limit effort="1.0" velocity="1.0" />
   </joint>
 
   <link name="link1">