Add symbolic support to Maliput lane

automotive/maliput/api/lane.cc

@@ -23,6 +23,12 @@ GeoPositionT<AutoDiffXd> Lane::ToGeoPositionT<AutoDiffXd>(
 }
 
 template<>
+GeoPositionT<symbolic::Expression> Lane::ToGeoPositionT<symbolic::Expression>(
+    const LanePositionT<symbolic::Expression>& lane_pos) const {
+  return DoToGeoPositionSymbolic(lane_pos);
+}
+
+template <>
 LanePositionT<double> Lane::ToLanePositionT<double>(
     const GeoPositionT<double>& geo_pos,
     GeoPositionT<double>* nearest_point,
@@ -64,6 +70,14 @@ LanePositionT<AutoDiffXd> Lane::ToLanePositionT<AutoDiffXd>(
   return result;
 }
 
+template <>
+LanePositionT<symbolic::Expression> Lane::ToLanePositionT<symbolic::Expression>(
+    const GeoPositionT<symbolic::Expression>& geo_pos,
+    GeoPositionT<symbolic::Expression>* nearest_point,
+    symbolic::Expression* distance) const {
+  return DoToLanePositionSymbolic(geo_pos, nearest_point, distance);
+}
+
 }  // namespace api
 }  // namespace maliput
 }  // namespace drake

automotive/maliput/api/lane.h

@@ -8,6 +8,7 @@
 #include "drake/common/autodiff.h"
 #include "drake/common/drake_copyable.h"
 #include "drake/common/drake_optional.h"
+#include "drake/common/symbolic.h"
 
 namespace drake {
 namespace maliput {
@@ -115,6 +116,7 @@ class Lane {
   /// Instantiated templates for the following kinds of T's are provided:
   /// - double
   /// - drake::AutoDiffXd
+  /// - drake::symbolic::Expression
   ///
   /// They are already available to link against in the containing library.
   ///
@@ -160,6 +162,7 @@ class Lane {
   /// Instantiated templates for the following kinds of T's are provided:
   /// - double
   /// - drake::AutoDiffXd
+  /// - drake::symbolic::Expression
   ///
   /// They are already available to link against in the containing library.
   ///
@@ -300,6 +303,25 @@ class Lane {
         "but a Lane backend has not overridden its AutoDiffXd specialization.");
   }
 
+  // symbolic::Expression overload of DoToGeoPosition().
+  virtual GeoPositionT<symbolic::Expression> DoToGeoPositionSymbolic(
+      const LanePositionT<symbolic::Expression>&) const {
+    throw std::runtime_error(
+        "DoToGeoPosition has been instantiated with symbolic::Expression "
+        "arguments, but a Lane backend has not overridden its "
+        "symbolic::Expression specialization.");
+  }
+
+  // symbolic::Expression overload of DoToLanePosition().
+  virtual LanePositionT<symbolic::Expression> DoToLanePositionSymbolic(
+      const GeoPositionT<symbolic::Expression>&,
+      GeoPositionT<symbolic::Expression>*, symbolic::Expression*) const {
+    throw std::runtime_error(
+        "DoToLanePosition has been instantiated with symbolic::Expression "
+        "arguments, but a Lane backend has not overridden its "
+        "symbolic::Expression specialization.");
+  }
+
   // TODO(jadecastro): Template the entire `api::Lane` class to prevent explicit
   // virtual functions for each member function.
   ///@}

automotive/maliput/dragway/lane.cc

@@ -102,6 +102,12 @@ api::GeoPositionT<AutoDiffXd> Lane::DoToGeoPositionAutoDiff(
           lane_pos.h()};
 }
 
+api::GeoPositionT<symbolic::Expression> Lane::DoToGeoPositionSymbolic(
+    const api::LanePositionT<symbolic::Expression>& lane_pos) const {
+  return {lane_pos.s(), lane_pos.r() + symbolic::Expression(Lane::y_offset()),
+          lane_pos.h()};
+}
+
 api::Rotation Lane::DoGetOrientation(
     const api::LanePosition&) const {
   return api::Rotation();  // Default is Identity.
@@ -121,6 +127,14 @@ api::LanePositionT<AutoDiffXd> Lane::DoToLanePositionAutoDiff(
   return ImplDoToLanePositionT<AutoDiffXd>(geo_pos, nearest_point, distance);
 }
 
+api::LanePositionT<symbolic::Expression> Lane::DoToLanePositionSymbolic(
+    const api::GeoPositionT<symbolic::Expression>& geo_pos,
+    api::GeoPositionT<symbolic::Expression>* nearest_point,
+    symbolic::Expression* distance) const {
+  return ImplDoToLanePositionT<symbolic::Expression>(geo_pos, nearest_point,
+                                                     distance);
+}
+
 template <typename T>
 api::LanePositionT<T> Lane::ImplDoToLanePositionT(
     const api::GeoPositionT<T>& geo_pos,
@@ -169,11 +183,14 @@ api::LanePositionT<T> Lane::ImplDoToLanePositionT(
     // implementation chooses the derivatives taken from within the interior
     // (zero) in order to remain consistent with the derivatives of
     // nearest_point and the returned LanePositionT.
-    if (distance_unsat > T(0.)) {
-      *distance = distance_unsat;
-    } else {  // ∂/∂x(distance) = 0 when distance = 0.
-      *distance = T(0.);
-    }
+    //
+    // We want to make sure that ∂/∂x(distance) = 0 when distance = 0 (not
+    // ∂/∂x(distance) = ∂/∂x(distance_unsat). The max function in
+    // common/autodiffxd.h returns the first argument when the two arguments
+    // have the same value. As a result, one should not change the order of the
+    // arguments in the max function below.
+    using std::max;
+    *distance = max(T(0.), distance_unsat);
   }
 
   return {closest_point.x(),

automotive/maliput/dragway/lane.h

@@ -164,6 +164,10 @@ class Lane final : public api::Lane {
   api::Rotation DoGetOrientation(const api::LanePosition& lane_pos) const
       final;
 
+  api::GeoPositionT<symbolic::Expression> DoToGeoPositionSymbolic(
+      const api::LanePositionT<symbolic::Expression>& lane_pos) const final;
+
+
   api::LanePosition DoToLanePosition(const api::GeoPosition& geo_pos,
                                      api::GeoPosition* nearest_point,
                                      double* distance) const final;
@@ -173,6 +177,11 @@ class Lane final : public api::Lane {
       api::GeoPositionT<AutoDiffXd>* nearest_point,
       AutoDiffXd* distance) const final;
 
+  api::LanePositionT<symbolic::Expression> DoToLanePositionSymbolic(
+      const api::GeoPositionT<symbolic::Expression>& geo_pos,
+      api::GeoPositionT<symbolic::Expression>* nearest_point,
+      symbolic::Expression* distance) const final;
+
   template <typename T>
   api::LanePositionT<T> ImplDoToLanePositionT(
       const api::GeoPositionT<T>& geo_pos,

automotive/maliput/dragway/test/dragway_test.cc

@@ -826,6 +826,127 @@ TEST_F(MaliputDragwayLaneTest, ThrowIfLanePosHasMismatchedDerivatives) {
       std::runtime_error);
 }
 
+TEST_F(MaliputDragwayLaneTest, ToLanePositionSymbolic1) {
+  // We want to show that the result from ToLanePositionSymbolic() corresponds
+  // with the one from ToLanePosition() when x, y, z are instantiated with the
+  // same values used to construct the input to ToLanePosition().
+
+  MakeDragway(1 /* num lanes */);
+  const symbolic::Variable x{"x"};
+  const symbolic::Variable y{"y"};
+  const symbolic::Variable z{"z"};
+  const double v_x = 1.0;
+  const double v_y = 2.0;
+  const double v_z = -0.5;
+  symbolic::Environment env{{{x, v_x}, {y, v_y}, {z, v_z}}};
+
+  // Computes symbolic gp and distance.
+  api::GeoPositionT<symbolic::Expression> symbolic_gp;
+  symbolic::Expression symbolic_distance{0.0};
+  const api::LanePositionT<symbolic::Expression> symbolic_lp{
+      lane_->ToLanePositionT<symbolic::Expression>(
+          api::GeoPositionT<symbolic::Expression>{x, y, z}, &symbolic_gp,
+          &symbolic_distance)};
+
+  // Computes gp and distance (of double).
+  api::GeoPosition gp;
+  double distance{0.0};
+  const api::LanePosition lp{
+      lane_->ToLanePosition(api::GeoPosition{v_x, v_y, v_z}, &gp, &distance)};
+
+  EXPECT_EQ(symbolic_lp.s().Evaluate(env), lp.s());
+  EXPECT_EQ(symbolic_lp.r().Evaluate(env), lp.r());
+  EXPECT_EQ(symbolic_lp.h().Evaluate(env), lp.h());
+
+  EXPECT_EQ(symbolic_gp.x().Evaluate(env), gp.x());
+  EXPECT_EQ(symbolic_gp.y().Evaluate(env), gp.y());
+  EXPECT_EQ(symbolic_gp.z().Evaluate(env), gp.z());
+  EXPECT_EQ(symbolic_distance.Evaluate(env), distance);
+
+  // These values (1.0, 2.0, -0.5) lie outside the lane. Therefore, 1) we have a
+  // positive distance and 2) the symbolic_gp evaluated with those values is
+  // different from (1.0, 2.0, -0.5).
+  EXPECT_GT(distance, 0.0);
+  EXPECT_FALSE(symbolic_gp.x().Evaluate(env) == v_x &&
+               symbolic_gp.y().Evaluate(env) == v_y &&
+               symbolic_gp.z().Evaluate(env) == v_z);
+}
+
+TEST_F(MaliputDragwayLaneTest, ToLanePositionSymbolic2) {
+  // We want to show that the result from ToLanePositionSymbolic() corresponds
+  // with the one from ToLanePosition() when x, y, z are instantiated with the
+  // same values used to construct the input to ToLanePosition().
+
+  MakeDragway(1 /* num lanes */);
+  const symbolic::Variable x{"x"};
+  const symbolic::Variable y{"y"};
+  const symbolic::Variable z{"z"};
+  const double v_x = 1.0;
+  const double v_y = 2.0;
+  const double v_z = 0.0;
+  symbolic::Environment env{{{x, v_x}, {y, v_y}, {z, v_z}}};
+
+  // Computes symbolic gp and distance.
+  api::GeoPositionT<symbolic::Expression> symbolic_gp;
+  symbolic::Expression symbolic_distance{0.0};
+
+  const api::LanePositionT<symbolic::Expression> symbolic_lp{
+      lane_->ToLanePositionT<symbolic::Expression>(
+          api::GeoPositionT<symbolic::Expression>{x, y, z}, &symbolic_gp,
+          &symbolic_distance)};
+
+  // Computes gp and distance (of double).
+  api::GeoPosition gp;
+  double distance{0.0};
+  const api::LanePosition lp{
+      lane_->ToLanePosition(api::GeoPosition{v_x, v_y, v_z}, &gp, &distance)};
+
+  EXPECT_EQ(symbolic_lp.s().Evaluate(env), lp.s());
+  EXPECT_EQ(symbolic_lp.r().Evaluate(env), lp.r());
+  EXPECT_EQ(symbolic_lp.h().Evaluate(env), lp.h());
+
+  EXPECT_EQ(symbolic_gp.x().Evaluate(env), gp.x());
+  EXPECT_EQ(symbolic_gp.y().Evaluate(env), gp.y());
+  EXPECT_EQ(symbolic_gp.z().Evaluate(env), gp.z());
+  EXPECT_EQ(symbolic_distance.Evaluate(env), distance);
+
+  // These values (1.0, 2.0, 0.0) lie on the lane. Therefore we have 1) a zero
+  // distance and 2) the symbolic_gp evaluated with those values is (1.0, 2.0,
+  // 0.0).
+  EXPECT_EQ(distance, 0.0);
+  EXPECT_TRUE(symbolic_gp.x().Evaluate(env) == v_x &&
+              symbolic_gp.y().Evaluate(env) == v_y &&
+              symbolic_gp.z().Evaluate(env) == v_z);
+}
+
+TEST_F(MaliputDragwayLaneTest, ToGeoPositionSymbolic) {
+  // We want to show that the result from ToGeoPositionSymbolic() corresponds
+  // with the one from ToGeoPosition() when s, r, h are instantiated with the
+  // same values used to construct the input to ToGeoPosition().
+  MakeDragway(1 /* num lanes */);
+
+  const symbolic::Variable s{"s"};
+  const symbolic::Variable r{"r"};
+  const symbolic::Variable h{"h"};
+  const double v_s = 1.0;
+  const double v_r = 2.0;
+  const double v_h = -0.5;
+  symbolic::Environment env{{{s, v_s}, {r, v_r}, {h, v_h}}};
+
+  // Computes symbolic gp.
+  const api::GeoPositionT<symbolic::Expression> symbolic_gp{
+      lane_->ToGeoPositionT<symbolic::Expression>(
+          api::LanePositionT<symbolic::Expression>{s, r, h})};
+
+  // Computes gp (of double).
+  const api::GeoPosition gp{
+      lane_->ToGeoPosition(api::LanePosition{v_s, v_r, v_h})};
+
+  EXPECT_EQ(symbolic_gp.x().Evaluate(env), gp.x());
+  EXPECT_EQ(symbolic_gp.y().Evaluate(env), gp.y());
+  EXPECT_EQ(symbolic_gp.z().Evaluate(env), gp.z());
+}
+
 }  // namespace
 }  // namespace dragway
 }  // namespace maliput

common/test/autodiffxd_max_test.cc

@@ -12,6 +12,17 @@ TEST_F(AutoDiffXdTest, Max) {
   CHECK_BINARY_FUNCTION_ADS_ADS(max, y, x, -0.4);
 }
 
+TEST_F(AutoDiffXdTest, TieBreakingCheck) {
+  // Given `max(v1, v2)`, Eigen autodiff's implementation of the max function
+  // and our overload return the first argument `v1` when `v1 == v2` holds. In
+  // Drake, we rely on this implementation-detail (i.e. Maliput Dragway's
+  // Lane::ImplDoToLanePositionT method). This test checks if the property holds
+  // so that we can detect a possible change in future.
+  const AutoDiffXd v1{1.0, Vector1<double>(1.)};
+  const AutoDiffXd v2{1.0, Vector1<double>(2.)};
+  EXPECT_EQ(max(v1, v2).derivatives()[0], 1.0);  // Returns v1, not v2.
+}
+
 }  // namespace
 }  // namespace test
 }  // namespace drake