Merge pull request RobotLocomotion#5374 from liangfok/feature/clamp

Moves saturate() into a shared location.

drake/automotive/BUILD

@@ -181,6 +181,7 @@ drake_cc_library(
         "//drake/common:cond",
         "//drake/common:double",
         "//drake/common:symbolic",
+        "//drake/math:saturate",
         "//drake/systems/rendering:frame_velocity",
         "//drake/systems/rendering:pose_vector",
     ],

drake/automotive/maliput/dragway/BUILD

@@ -25,6 +25,7 @@ drake_cc_library(
     deps = [
         "//drake/automotive/maliput/api",
         "//drake/common",
+        "//drake/math:saturate",
     ],
 )
 

drake/automotive/maliput/dragway/lane.cc

@@ -7,36 +7,14 @@
 #include "drake/automotive/maliput/dragway/road_geometry.h"
 #include "drake/automotive/maliput/dragway/segment.h"
 #include "drake/common/drake_assert.h"
+#include "drake/math/saturate.h"
 
 using std::make_unique;
 
 namespace drake {
 namespace maliput {
 namespace dragway {
 
-namespace {
-
-// Clamps the provided `value` by the provided `min` and `max` values. Returns
-// the clamped result.
-//
-// TODO(liang.fok) Once c++17 or later is used, switch to std::clamp().
-//
-// TODO(liang.fok) Move this and identical functions in dragway/road_geometry.cc
-// and SimpleCar into a common shared location.
-//
-double clamp(double value, double min, double max) {
-  double result = value;
-  if (value < min) {
-    result = min;
-  }
-  if (value > max) {
-    result = max;
-  }
-  return result;
-}
-
-}  // namespace
-
 Lane::Lane(const Segment* segment, const api::LaneId& id,  int index,
     double length, double y_offset, const api::RBounds& lane_bounds,
     const api::RBounds& driveable_bounds)
@@ -123,8 +101,8 @@ api::LanePosition Lane::DoToLanePosition(
   const double min_y = driveable_bounds_.r_min + y_offset_;
   const double max_y = driveable_bounds_.r_max + y_offset_;
 
-  const api::GeoPosition closest_point{clamp(geo_pos.x, min_x, max_x),
-                                       clamp(geo_pos.y, min_y, max_y),
+  const api::GeoPosition closest_point{math::saturate(geo_pos.x, min_x, max_x),
+                                       math::saturate(geo_pos.y, min_y, max_y),
                                        geo_pos.z};
   if (nearest_point != nullptr) {
     *nearest_point = closest_point;

drake/automotive/maliput/dragway/road_geometry.cc

@@ -7,33 +7,14 @@
 #include "drake/automotive/maliput/dragway/junction.h"
 #include "drake/common/drake_assert.h"
 #include "drake/common/text_logging.h"
+#include "drake/math/saturate.h"
 
 using std::make_unique;
 
 namespace drake {
 namespace maliput {
 namespace dragway {
 
-namespace {
-
-// Clamps the provided `value` by the provided `min` and `max` values. Returns
-// the clamped result.
-//
-// TODO(liang.fok) Once c++17 or later is used, switch to std::clamp().
-//
-double clamp(double value, double min, double max) {
-  double result = value;
-  if (value < min) {
-    result = min;
-  }
-  if (value > max) {
-    result = max;
-  }
-  return result;
-}
-
-}  // namespace
-
 RoadGeometry::RoadGeometry(const api::RoadGeometryId& id,
                int num_lanes,
                double length,
@@ -180,8 +161,8 @@ api::RoadPosition RoadGeometry::DoToRoadPosition(
       follows.
   */
   api::GeoPosition closest_position;
-  closest_position.x = clamp(geo_pos.x, min_x, max_x);
-  closest_position.y = clamp(geo_pos.y, min_y, max_y);
+  closest_position.x = math::saturate(geo_pos.x, min_x, max_x);
+  closest_position.y = math::saturate(geo_pos.y, min_y, max_y);
   closest_position.z = geo_pos.z;
 
   if (distance != nullptr) {

drake/automotive/simple_car.cc

@@ -12,6 +12,7 @@
 #include "drake/common/drake_assert.h"
 #include "drake/common/eigen_autodiff_types.h"
 #include "drake/common/symbolic_expression.h"
+#include "drake/math/saturate.h"
 #include "drake/systems/framework/vector_base.h"
 
 // This is used indirectly to allow DRAKE_ASSERT on symbolic::Expression.
@@ -157,18 +158,6 @@ void SimpleCar<T>::DoCalcTimeDerivatives(
   ImplCalcTimeDerivatives(config, *state, *input, rates);
 }
 
-namespace {
-// If value is within [low, high] then return it; else return the boundary.
-template <class T1, class T2, class T3>
-T1 saturate(const T1& value, const T2& low, const T3& high) {
-  DRAKE_ASSERT(low <= high);
-  return cond(
-      value < low, low,
-      value > high, high,
-      value);
-}
-}  // namespace
-
 template <typename T>
 void SimpleCar<T>::ImplCalcTimeDerivatives(const SimpleCarConfig<T>& config,
                                            const SimpleCarState<T>& state,
@@ -213,7 +202,7 @@ void SimpleCar<T>::ImplCalcTimeDerivatives(const SimpleCarConfig<T>& config,
   const T smooth_acceleration = damped_acceleration * smoothing_factor;
 
   // Determine steering.
-  const T saturated_steering_angle = saturate(
+  const T saturated_steering_angle = math::saturate(
       input.steering_angle(),
       -config.max_abs_steering_angle(),
       config.max_abs_steering_angle());

drake/math/BUILD

@@ -113,6 +113,18 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "saturate",
+    srcs = [],
+    hdrs = ["saturate.h"],
+    deps = [
+        "//drake/common",
+        # TODO(jwnimmer-tri) Remove once cond() no longer requires it.
+        "//drake/common:autodiff",
+        "//drake/common:cond",
+    ],
+)
+
 # === test/ ===
 
 drake_cc_googletest(
@@ -199,4 +211,13 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "saturate_test",
+    deps = [
+        ":saturate",
+        "//drake/common:autodiff",
+        "//drake/common:symbolic",
+    ],
+)
+
 cpplint()

drake/math/CMakeLists.txt

@@ -39,7 +39,8 @@ set(installed_headers
   roll_pitch_yaw_using_quaternion.h
   roll_pitch_yaw_not_using_quaternion.h
   rotation_conversion_gradient.h
-  rotation_matrix.h)
+  rotation_matrix.h
+  saturate.h)
 
 # Headers that are needed by code here but should not
 # be exposed anywhere else.

drake/math/saturate.h

@@ -0,0 +1,23 @@
+#pragma once
+
+// TODO(jwnimmer-tri): Figure out how to remove this include.
+#include "drake/common/autodiff_overloads.h"
+#include "drake/common/cond.h"
+#include "drake/common/drake_assert.h"
+
+namespace drake {
+namespace math {
+
+/// Saturates the input `value` between upper and lower bounds. If `value` is
+/// within `[low, high]` then return it; else return the boundary.
+template <class T1, class T2, class T3>
+T1 saturate(const T1& value, const T2& low, const T3& high) {
+  DRAKE_ASSERT(low <= high);
+  return cond(
+      value < low, low,
+      value > high, high,
+      value);
+}
+
+}  // namespace math
+}  // namespace drake

drake/math/test/saturate_test.cc

@@ -0,0 +1,86 @@
+#include "drake/math/saturate.h"
+
+#include <Eigen/Dense>
+#include <unsupported/Eigen/AutoDiff>
+
+#include "gtest/gtest.h"
+
+#include "drake/common/autodiff_overloads.h"
+#include "drake/common/eigen_autodiff_types.h"
+#include "drake/common/symbolic_environment.h"
+#include "drake/common/symbolic_expression.h"
+#include "drake/common/symbolic_expression_cell.h"
+#include "drake/common/symbolic_variable.h"
+
+namespace drake {
+
+using symbolic::Environment;
+using symbolic::Expression;
+
+namespace math {
+namespace {
+
+// Tests that saturate works with double types.
+GTEST_TEST(SaturateTest, DoubleTest) {
+  const double kLow{5};
+  const double kHigh{10};
+
+  const double kTooLow{kLow - 1e-10};
+  const double kMiddle{kLow + kHigh / 2.0};
+  const double kTooHigh{kHigh + 1e-10};
+  EXPECT_EQ(saturate(kTooLow, kLow, kHigh), kLow);
+  EXPECT_EQ(saturate(kMiddle, kLow, kHigh), kMiddle);
+  EXPECT_EQ(saturate(kTooHigh, kLow, kHigh), kHigh);
+}
+
+// Tests that saturate works with AutoDiff types.
+GTEST_TEST(SaturateTest, AutoDiffXdTest) {
+  const AutoDiffXd lowerbound{5.0, Vector1d(1.5)};
+  const AutoDiffXd upperbound{10.0, Vector1d(-1.5)};
+  const AutoDiffXd withinBoundsValue{7.5, Vector1d(6.38)};
+
+  EXPECT_EQ(
+      saturate(AutoDiffXd{1.0, Vector1d(10.2)}, lowerbound, upperbound),
+      lowerbound);
+  EXPECT_EQ(
+      saturate(withinBoundsValue, lowerbound, upperbound),
+      withinBoundsValue);
+  EXPECT_EQ(
+      saturate(AutoDiffXd{100.3, Vector1d(-9)}, lowerbound, upperbound),
+      upperbound);
+
+  // Tests a mixed-type scenario. Adding or subtracting 1 from an AutoDiffXd
+  // does not result in an AutoDiffXd, but rather some intermediate type that
+  // implicitly converts to AutoDiffXd.
+  EXPECT_EQ(
+      saturate(AutoDiffXd{1.0, Vector1d(10.2)}, lowerbound - 1, upperbound + 1),
+      lowerbound - 1);
+}
+
+// Tests that saturate() works with symbolic::Expression types.
+GTEST_TEST(SaturateTest, SymbolicExpressionTest) {
+  Expression result;
+  result = saturate(Expression{1.5}, Expression::One(), Expression::Pi());
+  EXPECT_EQ(result.to_string(), "1.5");
+  result = saturate(Expression::Zero(), Expression::One(), Expression::Pi());
+  EXPECT_EQ(result.to_string(), "1");
+  result = saturate(Expression{5.6}, Expression::One(), Expression::Pi());
+  const std::string kPi{"3.14"};
+  EXPECT_EQ(result.to_string().compare(0, kPi.length(), kPi), 0);
+
+  symbolic::Variable x{"x"};
+  symbolic::Variable lo{"lo"};
+  symbolic::Variable hi{"hi"};
+  auto saturate_expression = saturate(
+      Expression{x}, Expression{lo}, Expression{hi});
+  EXPECT_EQ(saturate_expression.Evaluate(
+      Environment{{x, 0}, {lo, 3}, {hi, 10}}), 3);
+  EXPECT_EQ(saturate_expression.Evaluate(
+      Environment{{x, 5}, {lo, 3}, {hi, 10}}), 5);
+  EXPECT_EQ(saturate_expression.Evaluate(
+      Environment{{x, 12.334}, {lo, 3}, {hi, 10}}), 10);
+}
+
+}  // namespace
+}  // namespace math
+}  // namespace drake

drake/systems/primitives/BUILD

@@ -133,6 +133,7 @@ drake_cc_library(
     srcs = ["saturation.cc"],
     hdrs = ["saturation.h"],
     deps = [
+        "//drake/math:saturate",
         "//drake/systems/framework",
     ],
 )

drake/systems/primitives/saturation.cc

@@ -6,6 +6,7 @@
 #include <limits>
 
 #include "drake/common/eigen_types.h"
+#include "drake/math/saturate.h"
 #include "drake/systems/framework/leaf_system.h"
 
 namespace drake {
@@ -92,12 +93,9 @@ void Saturation<T>::DoCalcOutput(const Context<T>& context,
 
   const auto& u = input_vector->get_value();
 
-  using std::min;
-  using std::max;
-
   // Loop through and set the saturation values.
   for (int i = 0; i < u_min.size(); ++i) {
-    y[i] = min(max(u[i], u_min[i]), u_max[i]);
+    y[i] = math::saturate(u[i], u_min[i], u_max[i]);
   }
 }