Fixes SDF parser so that default joint limits are plus/minus infinity.

ChristopherSweeney · Sep 25, 2018 · 5e05817 · 5e05817
1 parent e280692
commit 5e05817
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 11 deletions.
diff --git a/multibody/multibody_tree/multibody_plant/multibody_plant.cc b/multibody/multibody_tree/multibody_plant/multibody_plant.cc
@@ -467,10 +467,9 @@ void MultibodyPlant<T>::SetUpJointLimitsParameters() {
     const double penalty_time_scale = kAlpha * time_step();
 
     if (revolute_joint) {
-      // We only compute parameters if they are far from being infinity.
-      // SDF defaults to 1.0e16 instead of infinity.
-      if (-1.0e16 < revolute_joint->lower_limit() ||
-          revolute_joint->upper_limit() < 1.0e16) {
+      // We only compute parameters if joints do have upper/lower bounds.
+      if (!std::isinf(revolute_joint->lower_limit()) ||
+          !std::isinf(revolute_joint->upper_limit())) {
         joint_limits_parameters_.joints_with_limits.push_back(
             revolute_joint->index());
 
@@ -490,10 +489,9 @@ void MultibodyPlant<T>::SetUpJointLimitsParameters() {
     }
 
     if (prismatic_joint) {
-      // We only compute parameters if they are far from being infinity.
-      // SDF defaults to 1.0e16 instead of infinity.
-      if (-1.0e16 < prismatic_joint->lower_limit() ||
-          prismatic_joint->upper_limit() < 1.0e16) {
+      // We only compute parameters if joints do have upper/lower bounds.
+      if (!std::isinf(prismatic_joint->lower_limit()) ||
+          !std::isinf(prismatic_joint->upper_limit())) {
         joint_limits_parameters_.joints_with_limits.push_back(
             prismatic_joint->index());
 

diff --git a/multibody/multibody_tree/parsing/multibody_plant_sdf_parser.cc b/multibody/multibody_tree/parsing/multibody_plant_sdf_parser.cc
@@ -1,5 +1,6 @@
 #include "drake/multibody/multibody_tree/parsing/multibody_plant_sdf_parser.h"
 
+#include <limits>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -210,14 +211,23 @@ std::pair<double, double> ParseJointLimits(const sdf::Joint& joint_spec) {
     throw std::runtime_error(
         "An axis must be specified for joint '" + joint_spec.Name() + "'");
   }
-  const double lower_limit = axis->Lower();
-  const double upper_limit = axis->Upper();
+
+  // SDF defaults to ±1.0e16 for joints with no limits, see
+  // http://sdformat.org/spec?ver=1.6&elem=joint#axis_limit.
+  // Drake marks joints with no limits with ±numeric_limits<double>::infinity()
+  // and therefore we make the change here.
+  const double lower_limit =
+      axis->Lower() == -1.0e16 ?
+      -std::numeric_limits<double>::infinity() : axis->Lower();
+  const double upper_limit =
+      axis->Upper() == 1.0e16 ?
+      std::numeric_limits<double>::infinity() : axis->Upper();
   if (lower_limit > upper_limit) {
     throw std::runtime_error(
         "The lower limit must be lower (or equal) than the upper limit for "
         "joint '" + joint_spec.Name() + "'.");
   }
-  return std::make_pair(axis->Lower(), axis->Upper());
+  return std::make_pair(lower_limit, upper_limit);
 }
 
 // Helper method to add joints to a MultibodyPlant given an sdf::Joint

diff --git a/multibody/multibody_tree/parsing/test/multibody_plant_sdf_parser_test.cc b/multibody/multibody_tree/parsing/test/multibody_plant_sdf_parser_test.cc
@@ -96,6 +96,15 @@ class AcrobotModelTests : public ::testing::Test {
   std::unique_ptr<systems::Context<double>> benchmark_context_;
 };
 
+// Verifies that the default joint limits when no limits are specified in the
+// SDF file are ±infinity.
+TEST_F(AcrobotModelTests, DefaultJointLimits) {
+  EXPECT_TRUE(std::isinf(shoulder_->lower_limit()));
+  EXPECT_TRUE(std::isinf(shoulder_->upper_limit()));
+  EXPECT_TRUE(std::isinf(elbow_->lower_limit()));
+  EXPECT_TRUE(std::isinf(elbow_->upper_limit()));
+}
+
 // This test verifies a number of invariants such as model sizes and that body
 // and joint models were properly added.
 TEST_F(AcrobotModelTests, ModelBasics) {