trajectories: construct HermitianDenseOutput from PiecewisePolynomial (…

…RobotLocomotion#13006)

now with support for default scalars.
(I should have done this version in RobotLocomotion#12957, but didn't realize it until I was able to push forward the default scalar support for PiecewisePolynomail).  This technically changes the API that I introduced last week; i think it's safe to assume that nobody was using it yet.

systems/analysis/hermitian_dense_output.h

@@ -19,13 +19,14 @@ namespace systems {
 
 namespace internal {
 
-/// Converts a matrix with scalar type `S` elements to a matrix with double
-/// type elements, failing at runtime if the type cannot be converted.
+/// Returns @p input_matrix as an Eigen::Matrix<double, ...> with the same size
+/// allocation as @p input_matrix.  Calls ExtractDoubleOrThrow on each element
+/// of the matrix, and therefore throws if any one of the extractions fail.
 /// @see ExtractDoubleOrThrow(const T&)
-/// @tparam S A valid Eigen scalar type.
-template <typename S>
-MatrixX<double> ExtractDoublesOrThrow(const MatrixX<S>& input_matrix) {
-  return input_matrix.unaryExpr([] (const S& value) {
+template <typename Derived>
+MatrixX<double> ExtractDoublesOrThrow(
+    const Eigen::MatrixBase<Derived>& input_matrix) {
+  return input_matrix.unaryExpr([] (const typename Derived::Scalar& value) {
       return ExtractDoubleOrThrow(value);
   });
 }
@@ -241,10 +242,30 @@ class HermitianDenseOutput final : public StepwiseDenseOutput<T> {
 
   /// Initialize the DenseOutput with an existing trajectory.
   explicit HermitianDenseOutput(
-      const trajectories::PiecewisePolynomial<double>& trajectory)
+      const trajectories::PiecewisePolynomial<T>& trajectory)
       : start_time_(trajectory.start_time()),
-        end_time_(trajectory.end_time()),
-        continuous_trajectory_(trajectory) {}
+        end_time_(trajectory.end_time()) {
+    if constexpr (std::is_same<T, double>::value) {
+      continuous_trajectory_ = trajectory;
+      return;
+    }
+
+    // Create continuous_trajectory_ by converting all the segments to double.
+    using trajectories::PiecewisePolynomial;
+    const std::vector<T>& breaks = trajectory.get_segment_times();
+    for (int i = 0; i < trajectory.get_number_of_segments(); i++) {
+      const typename PiecewisePolynomial<T>::PolynomialMatrix& poly =
+          trajectory.getPolynomialMatrix(i);
+      MatrixX<Polynomiald> polyd = poly.unaryExpr([](const Polynomial<T>& p) {
+        return Polynomiald(
+            internal::ExtractDoublesOrThrow(p.GetCoefficients()));
+      });
+      continuous_trajectory_.ConcatenateInTime(
+          PiecewisePolynomial<double>({polyd},
+                                      {ExtractDoubleOrThrow(breaks[i]),
+                                       ExtractDoubleOrThrow(breaks[i + 1])}));
+    }
+  }
 
   /// Update output with the given @p step.
   ///
@@ -401,7 +422,8 @@ class HermitianDenseOutput final : public StepwiseDenseOutput<T> {
 
   // TODO(hidmic): When PiecewisePolynomial supports scalar types other than
   // doubles, pass in the template parameter T to it too and remove all scalar
-  // type conversions.
+  // type conversions.  UPDATE(russt): New plan is to deprecate this class, as
+  // PiecewisePolynomial can serve the intended role by itself.
 
   // The underlying PiecewisePolynomial continuous trajectory.
   trajectories::PiecewisePolynomial<double> continuous_trajectory_{};

systems/analysis/test/hermitian_dense_output_test.cc

@@ -306,42 +306,63 @@ TYPED_TEST(HermitianDenseOutputTest, CorrectEvaluation) {
   const trajectories::PiecewisePolynomial<double> hermite_spline =
       trajectories::PiecewisePolynomial<double>::CubicHermite(
           spline_times, spline_states, spline_state_derivatives);
-  // Construct one version using the PiecewisePolynomial constructor.
-  HermitianDenseOutput<TypeParam> from_spline(hermite_spline);
-  // Build another version incrementally.
-  HermitianDenseOutput<TypeParam> incremental;
+  // Instantiates dense output.
+  HermitianDenseOutput<TypeParam> dense_output;
   // Updates output for the first time.
   typename HermitianDenseOutput<TypeParam>::IntegrationStep first_step(
       this->kInitialTime, this->kInitialState, this->kInitialStateDerivative);
   first_step.Extend(this->kMidTime, this->kMidState, this->kMidStateDerivative);
-  incremental.Update(first_step);
+  dense_output.Update(first_step);
   // Updates output a second time.
   typename HermitianDenseOutput<TypeParam>::IntegrationStep second_step(
       this->kMidTime, this->kMidState, this->kMidStateDerivative);
   second_step.Extend(this->kFinalTime, this->kFinalState,
                      this->kFinalStateDerivative);
-  incremental.Update(second_step);
+  dense_output.Update(second_step);
   // Consolidates all previous updates.
-  incremental.Consolidate();
+  dense_output.Consolidate();
   // Verifies that dense output and Hermite spline match.
   const double kAccuracy{1e-12};
-  EXPECT_FALSE(from_spline.is_empty());
-  EXPECT_FALSE(incremental.is_empty());
-  EXPECT_EQ(from_spline.start_time(), incremental.start_time());
-  EXPECT_EQ(from_spline.end_time(), incremental.end_time());
+  EXPECT_FALSE(dense_output.is_empty());
   for (TypeParam t = this->kInitialTime;
        t <= this->kFinalTime; t += this->kTimeStep) {
     const MatrixX<double> matrix_value =
         hermite_spline.value(ExtractDoubleOrThrow(t));
     const VectorX<TypeParam> vector_value =
         matrix_value.col(0).template cast<TypeParam>();
-    EXPECT_TRUE(CompareMatrices(from_spline.Evaluate(t),
-                                vector_value, kAccuracy));
-    EXPECT_TRUE(CompareMatrices(incremental.Evaluate(t),
+    EXPECT_TRUE(CompareMatrices(dense_output.Evaluate(t),
                                 vector_value, kAccuracy));
   }
 }
 
+// Construct a HermitianDenseOutput<T> from PiecewisePolynomial<U>.
+template <typename T>
+void TestScalarType() {
+  const Vector3<T> breaks(0.0, 1.0, 2.0);
+  const RowVector3<T> samples(6.0, 5.0, 4.0);
+
+  const auto foh =
+      trajectories::PiecewisePolynomial<T>::FirstOrderHold(breaks, samples);
+
+  const HermitianDenseOutput<T> hdo(foh);
+
+  EXPECT_EQ(ExtractDoubleOrThrow(hdo.start_time()),
+            ExtractDoubleOrThrow(breaks(0)));
+  EXPECT_EQ(ExtractDoubleOrThrow(hdo.end_time()),
+            ExtractDoubleOrThrow(breaks(2)));
+
+  for (const T& time : {0.1, 0.4, 1.6}) {
+    EXPECT_NEAR(ExtractDoubleOrThrow(hdo.Evaluate(time)(0)),
+                ExtractDoubleOrThrow(foh.value(time)(0)), 1e-14);
+  }
+}
+
+GTEST_TEST(HermitianDenseOutputTest, ConstructFromPiecewisePolynomialTest) {
+  TestScalarType<double>();
+  TestScalarType<AutoDiffXd>();
+  TestScalarType<symbolic::Expression>();
+}
+
 }  // namespace
 }  // namespace analysis
 }  // namespace systems