Merge pull request RobotLocomotion#9270 from ekumenlabs/Issue/Multila…

…ne_Technical_Debt_Reverse_Endpoint_Continuity

Improve explicit theta_dot treatment in Multilane's Builder

automotive/maliput/multilane/BUILD.bazel

@@ -112,6 +112,7 @@ drake_cc_googletest(
         ":builder",
         "//automotive/maliput/api/test_utilities",
         "//automotive/maliput/multilane/test_utilities",
+        "//common:essential",
         "//common/test_utilities:eigen_matrix_compare",
     ],
 )

automotive/maliput/multilane/builder.cc

@@ -142,6 +142,14 @@ const Connection* Builder::Connect(const std::string& id,
   return connections_.back().get();
 }
 
+// TODO(hidmic): Support connections where the constant C in the
+//               continuity constraints theta_dotA - sin(betaA) = C
+//               and theta_dotB - sin(betaB) = C is non-zero. This
+//               is necessary to ensure continuity when arbitrary
+//               theta_dot values are provided by the user. Then,
+//               do not clear *explicitly* set theta_dot values
+//               upon Spec construction using a Connection anymore.
+
 const Connection* Builder::Connect(const std::string& id,
                                    const LaneLayout& lane_layout,
                                    const StartLane::Spec& start_spec,
@@ -164,16 +172,16 @@ const Connection* Builder::Connect(const std::string& id,
       -lane_layout.ref_r0() +
       lane_width_ *
           static_cast<double>(start_spec.lane_id() - lane_layout.ref_lane());
-  // Computes the displacement vector from lane point to the reference curve
-  // point.
-  const Vector3<double> start_lane_to_ref(
-      -start_lane_offset * std::cos(start_superelevation) *
-          std::sin(start_heading),
-      start_lane_offset * std::cos(start_superelevation) *
-          std::cos(start_heading),
-      start_lane_offset * std::sin(start_superelevation));
+
+  // Gets the rotation matrix at the start lane endpoint and translates the
+  // point to start_reference_position.
+  const api::Rotation start_rotation = api::Rotation::FromRpy(
+      start_superelevation, -std::atan(start_spec.endpoint().z().z_dot()),
+      start_heading);
   const Vector3<double> start_reference_position =
-      start_lane_position - start_lane_to_ref;
+      start_lane_position +
+      start_rotation.matrix() * Vector3<double>(0., -start_lane_offset, 0.);
+
   // Assigns the start endpoint.
   Endpoint start{
       EndpointXy(start_reference_position.x(), start_reference_position.y(),
@@ -217,14 +225,10 @@ const Connection* Builder::Connect(const std::string& id,
   const double curvature =
       std::copysign(1., arc_offset.d_theta()) / arc_offset.radius();
 
-  // Fills arc related parameters, computes end Endpoint and creates the
-  // RoadCurve.
-  const double theta0 = start_spec.endpoint().xy().heading() -
-                        std::copysign(M_PI / 2., arc_offset.d_theta());
   // Gets the initial heading and superelevation.
   const double start_superelevation = start_spec.endpoint().z().theta();
 
-  // Computes the center.
+  // Computes the start_lane_radius to scale z_dot.
   const double start_lane_offset =
       -lane_layout.ref_r0() +
       lane_width_ *
@@ -233,20 +237,26 @@ const Connection* Builder::Connect(const std::string& id,
                                    start_lane_offset *
                                        std::cos(start_superelevation) *
                                        std::copysign(1., arc_offset.d_theta());
-  const double cx =
-      start_spec.endpoint().xy().x() - std::cos(theta0) * start_lane_radius;
-  const double cy =
-      start_spec.endpoint().xy().y() - std::sin(theta0) * start_lane_radius;
   const double start_z_dot = start_spec.endpoint().z().z_dot() *
                              start_lane_radius / arc_offset.radius();
+
+  // Gets the rotation matrix at the start lane endpoint and translates the
+  // point to start_reference_position.
+  const api::Rotation start_rotation = api::Rotation::FromRpy(
+      start_superelevation, -std::atan(start_z_dot),
+      start_spec.endpoint().xy().heading());
+  const Vector3<double> start_lane_position{start_spec.endpoint().xy().x(),
+                                            start_spec.endpoint().xy().y(),
+                                            start_spec.endpoint().z().z()};
+  const Vector3<double> start_reference_position =
+      start_lane_position +
+      start_rotation.matrix() * Vector3<double>(0., -start_lane_offset, 0.);
+
   // Assigns the start endpoint.
   Endpoint start{
-      EndpointXy(cx + arc_offset.radius() * std::cos(theta0),
-                 cy + arc_offset.radius() * std::sin(theta0),
+      EndpointXy(start_reference_position.x(), start_reference_position.y(),
                  start_spec.endpoint().xy().heading()),
-      EndpointZ(start_spec.endpoint().z().z() -
-                    start_lane_offset * std::sin(start_superelevation) *
-                        std::copysign(1., arc_offset.d_theta()),
+      EndpointZ(start_reference_position.z(),
                 start_z_dot, start_spec.endpoint().z().theta(), {})};
   ComputeContinuityConstraint(curvature, &(start.get_mutable_z()));
 
@@ -301,23 +311,108 @@ Group* Builder::MakeGroup(const std::string& id,
 
 
 namespace {
-// Determine the heading (in xy-plane) along the centerline when
-// travelling towards/into the lane, from the specified end.
-double HeadingIntoLane(const api::Lane* const lane,
-                       const api::LaneEnd::Which end) {
+
+// Determine the heading direction at an `r_offset` leftwards of the `lane`
+// centerline when traveling outwards from the specified `end`.
+Vector3<double> DirectionOutFromLane(const api::Lane* const lane,
+                                     const api::LaneEnd::Which end,
+                                     double r_offset) {
+  const Vector3<double> s_hat = Vector3<double>::UnitX();
   switch (end) {
     case api::LaneEnd::kStart: {
-      return lane->GetOrientation({0., 0., 0.}).yaw();
+      return -lane->GetOrientation(
+          {0., -r_offset, 0.}).matrix() * s_hat;
     }
     case api::LaneEnd::kFinish: {
-      return lane->GetOrientation({lane->length(), 0., 0.}).yaw() + M_PI;
+      return lane->GetOrientation(
+          {lane->length(), r_offset, 0.}).matrix() * s_hat;
     }
     default: { DRAKE_ABORT(); }
   }
 }
-}  // namespace
 
 
+// Checks if the heading direction of the all the given lanes at an `r_offset`
+// leftwards of the centerlines when traveling outwards from their specified
+// end in `set` matches the reference `direction_at_r_offset`, down to the
+// given `angular_tolerance`.
+bool AreLanesDirectionsWithinTolerance(
+    const Vector3<double>& direction_at_r_offset, double r_offset,
+    const api::LaneEndSet* set, double angular_tolerance) {
+  DRAKE_DEMAND(set != nullptr);
+
+  for (int i = 0; i < set->size(); ++i) {
+    const api::LaneEnd& le = set->get(i);
+    const Vector3<double> other_direction =
+        DirectionOutFromLane(le.lane, le.end, r_offset);
+    const double angular_deviation =
+        std::acos(direction_at_r_offset.dot(other_direction));
+    if (angular_deviation > angular_tolerance) {
+      return false;
+    }
+  }
+  return true;
+}
+
+
+// Checks if the `lane` is G1 continuous along an `r_offset` leftwards of
+// the `lane` centerline when traveling outwards from the given `end`, at the
+// branch point and down to the given `angular_tolerance`. `lane` is assumed
+// to have an out-of-the-lane tangent vector that is coincident with those of
+// the lanes in the `parallel_set`, which in turn are anti-parallel to
+// those of the lanes in the `antiparallel_set`.
+bool IsLaneContinuousAtBranchPointAlongROffset(
+    const api::Lane* lane, const api::LaneEnd::Which end,
+    const api::LaneEndSet* parallel_set,
+    const api::LaneEndSet* antiparallel_set,
+    double r_offset, double angular_tolerance) {
+  DRAKE_DEMAND(lane != nullptr);
+  DRAKE_DEMAND(parallel_set != nullptr);
+  DRAKE_DEMAND(antiparallel_set != nullptr);
+
+  const Vector3<double> direction =
+      DirectionOutFromLane(lane, end, r_offset);
+  return
+      (AreLanesDirectionsWithinTolerance(
+          direction, r_offset,
+          parallel_set, angular_tolerance) &&
+       AreLanesDirectionsWithinTolerance(
+           -direction, r_offset,
+           antiparallel_set, angular_tolerance));
+}
+
+
+// Checks if the `lane` is G1 continuous at the branchpoint on the given
+// `end`, down to an `angular_tolerance`. `lane` is assumed to have an
+// out-of-the-lane tangent vector that is coincident with those of the
+// lanes in the `parallel_set`, which in turn are anti-parallel to those
+// of the lanes in the `antiparallel_set`.
+bool IsLaneContinuousAtBranchPoint(
+    const api::Lane* lane, const api::LaneEnd::Which end,
+    const api::LaneEndSet* parallel_set,
+    const api::LaneEndSet* antiparallel_set,
+    double angular_tolerance) {
+  DRAKE_DEMAND(lane != nullptr);
+  DRAKE_DEMAND(parallel_set != nullptr);
+  DRAKE_DEMAND(antiparallel_set != nullptr);
+
+  constexpr double kZeroROffset{0.};
+  const double s_at_branch_point =
+      (end == api::LaneEnd::kFinish) ? lane->length() : 0.;
+  const double max_r_offset =
+      lane->lane_bounds(s_at_branch_point).max();
+  return
+      (IsLaneContinuousAtBranchPointAlongROffset(
+          lane, end, parallel_set, antiparallel_set,
+          kZeroROffset, angular_tolerance) &&
+       IsLaneContinuousAtBranchPointAlongROffset(
+           lane, end, parallel_set, antiparallel_set,
+           max_r_offset, angular_tolerance));
+}
+
+
+}  // namespace
+
 
 BranchPoint* Builder::FindOrCreateBranchPoint(
     const Endpoint& point,
@@ -363,21 +458,33 @@ void Builder::AttachBranchPoint(
     bp->AddABranch({lane, end});
     return;
   }
+
   // Otherwise, assess if this new lane-end is parallel or anti-parallel to
   // the first lane-end.  Parallel: go to same, A-side; anti-parallel:
   // other, B-side.  Do this by examining the dot-product of the heading
   // vectors (rather than goofing around with cyclic angle arithmetic).
-  const double new_h = HeadingIntoLane(lane, end);
-  const api::LaneEnd old_le = bp->GetASide()->get(0);
-  const double old_h = HeadingIntoLane(old_le.lane, old_le.end);
-  if (((std::cos(new_h) * std::cos(old_h)) +
-       (std::sin(new_h) * std::sin(old_h))) > 0.) {
+  constexpr double kZeroROffset{0.};
+  const Vector3<double> direction =
+      DirectionOutFromLane(lane, end, kZeroROffset);
+  const api::LaneEnd& old_le = bp->GetASide()->get(0);
+  const Vector3<double> old_direction =
+      DirectionOutFromLane(old_le.lane, old_le.end, kZeroROffset);
+  if (direction.dot(old_direction) > 0.) {
+    // Assert continuity before attaching the lane.
+    DRAKE_THROW_UNLESS(IsLaneContinuousAtBranchPoint(
+        lane, end, bp->GetASide(), bp->GetBSide(),
+        angular_tolerance_));
     bp->AddABranch({lane, end});
   } else {
+    // Assert continuity before attaching the lane.
+    DRAKE_THROW_UNLESS(IsLaneContinuousAtBranchPoint(
+        lane, end, bp->GetBSide(), bp->GetASide(),
+        angular_tolerance_));
     bp->AddBBranch({lane, end});
   }
 }
 
+
 std::vector<Lane*> Builder::BuildConnection(
     const Connection* const conn, Junction* const junction,
     RoadGeometry* const road_geometry,

automotive/maliput/multilane/builder.h

@@ -59,9 +59,9 @@ class StartReference {
   }
 
   /// Builds a Spec at `connection`'s `end` side with `direction` direction.
-  /// When `direction` == `Direction::kReverse`, `endpoint` is reversed.
-  /// Spec's theta_dot will be cleared so the Builder adjusts it to match
-  /// continuity constraints.
+  /// `connection`'s theta_dot at the given `end` will be ignored by the new
+  /// Spec so that the Builder can adjust it to match road continuity
+  /// constraints.
   Spec at(const Connection& connection, api::LaneEnd::Which end,
           Direction direction) const {
     Endpoint endpoint = end == api::LaneEnd::Which::kStart ? connection.start()
@@ -121,7 +121,8 @@ class StartLane {
 
   /// Builds a Spec at `endpoint` with `direction` direction. When
   /// `direction` == `Direction::kReverse`, `endpoint` is reversed.
-  /// `endpoint`'s theta_dot must be nullopt.
+  /// Additionally, `endpoint`'s theta_dot must be nullopt. Otherwise
+  /// the Builder is unable to match road continuity constraints.
   Spec at(const Endpoint& endpoint, Direction direction) const {
     DRAKE_DEMAND(!endpoint.z().theta_dot().has_value());
     return direction == Direction::kForward
@@ -130,13 +131,12 @@ class StartLane {
   }
 
   /// Builds a Spec at `connection`'s `lane_id` lane at `end` side with
-  /// `direction` direction. When `direction` == `Direction::kReverse`,
-  /// `endpoint` is reversed.
+  /// `direction` direction. `connection`'s theta_dot at the given `end`
+  /// will be ignored by the new Spec so that the Builder can adjust it
+  /// to match road continuity constraints.
   ///
   /// `lane_id` must be non-negative and smaller than `connection`'s number of
   /// lanes.
-  /// Spec's theta_dot will be cleared so the Builder adjusts it to match
-  /// continuity constraints.
   Spec at(const Connection& connection, int lane_id, api::LaneEnd::Which end,
           Direction direction) const {
     DRAKE_DEMAND(lane_id >= 0 && lane_id < connection.num_lanes());
@@ -189,10 +189,9 @@ class EndReference {
   EndReference() = default;
 
   /// Builds a Spec at `connection`'s `end` side with `direction` direction.
-  /// When `direction` == `Direction::kReverse`, `end`-side endpoint's
-  /// EndpointZ is reversed.
-  /// Spec's theta_dot will be cleared so the Builder adjusts it to match
-  /// continuity constraints.
+  /// `connection`'s theta_dot at the given `end` will be ignored by the new
+  /// Spec so that the Builder can adjust it to match road continuity
+  /// constraints.
   Spec z_at(const Connection& connection, api::LaneEnd::Which end,
             Direction direction) const {
     EndpointZ endpoint_z = end == api::LaneEnd::Which::kStart
@@ -258,13 +257,12 @@ class EndLane {
   }
 
   /// Builds a Spec at `connection`'s `end` side with `direction` direction.
-  /// When `direction` == `Direction::kReverse`, `end`-side endpoint's
-  /// EndpointZ is reversed.
+  /// `connection`'s theta_dot at the given `end` will be ignored by the new
+  /// Spec so that the Builder can adjust it to match road continuity
+  /// constraints.
   ///
   /// `lane_id` must be non-negative and smaller than `connection`'s number of
   /// lanes.
-  /// Spec's theta_dot will be cleared so the Builder adjusts it to match
-  /// continuity constraints.
   Spec z_at(const Connection& connection, int lane_id, api::LaneEnd::Which end,
             Direction direction) const {
     DRAKE_DEMAND(lane_id >= 0 && lane_id < connection.num_lanes());
@@ -279,7 +277,8 @@ class EndLane {
 
   /// Builds an Spec at `endpoint_z` with `direction` direction.
   /// When `direction` == `Direction::kReverse`, `endpoint_z` is reversed.
-  /// `endpoint_z`'s theta_dot must be nullopt.
+  /// Additionally, `endpoint`'s theta_dot must be nullopt. Otherwise
+  /// the Builder is unable to match road continuity constraints.
   Spec z_at(const EndpointZ& endpoint_z, Direction direction) const {
     DRAKE_DEMAND(!endpoint_z.theta_dot().has_value());
     return direction == Direction::kForward
@@ -471,6 +470,8 @@ class BuilderBase {
       const std::vector<const Connection*>& connections) = 0;
 
   /// Produces a RoadGeometry, with the ID `id`.
+  /// @throws std::runtime_error if unable to produce a valid
+  ///                            (i.e. G1) RoadGeometry.
   virtual std::unique_ptr<const api::RoadGeometry> Build(
       const api::RoadGeometryId& id) const = 0;
 };
@@ -524,6 +525,16 @@ class BuilderFactoryBase {
 /// the Segments associated with the grouped Connections; ungrouped
 /// Connections each receive their own Junction.
 ///
+/// At Endpoints where theta_dot (i.e. the rate of change of superelevation with
+/// respect to an arc-length coordinate t that travels along the Connection
+/// curve’s projection over the z = 0 plane) has not been explicitly specified
+/// the Builder will automatically calculate theta_dot values to preserve G1
+/// continuity across Connections. theta_dot values can only be explicitly
+/// specified for a Connection created from a reference curve with an explicit
+/// start or end Endpoint. However, specifying an explicit theta_dot may cause
+/// Builder::Build() to throw if it cannot assemble a G1 continuous
+/// RoadGeometry.
+///
 /// Specific suffixes are used to name Maliput entities. The following list
 /// explains the naming convention:
 ///