Support default scalars for TrajectorySource

bindings/pydrake/systems/primitives_py.cc

@@ -594,6 +594,15 @@ PYBIND11_MODULE(primitives, m) {
                 .doc_3args_period_sec_abstract_model_value_offset_sec)
         .def("period", &ZeroOrderHold<T>::period, doc.ZeroOrderHold.period.doc)
         .def("offset", &ZeroOrderHold<T>::offset, doc.ZeroOrderHold.offset.doc);
+
+    DefineTemplateClassWithDefault<TrajectorySource<T>, LeafSystem<T>>(
+        m, "TrajectorySource", GetPyParam<T>(), doc.TrajectorySource.doc)
+        .def(py::init<const trajectories::Trajectory<T>&, int, bool>(),
+            py::arg("trajectory"), py::arg("output_derivative_order") = 0,
+            py::arg("zero_derivatives_beyond_limits") = true,
+            doc.TrajectorySource.ctor.doc)
+        .def("UpdateTrajectory", &TrajectorySource<T>::UpdateTrajectory,
+            py::arg("trajectory"), doc.TrajectorySource.UpdateTrajectory.doc);
   };
   type_visit(bind_common_scalar_types, CommonScalarPack{});
 
@@ -743,15 +752,6 @@ PYBIND11_MODULE(primitives, m) {
           py::arg("num_outputs"), py::arg("sampling_interval_sec"),
           doc.RandomSource.ctor.doc);
 
-  py::class_<TrajectorySource<double>, LeafSystem<double>>(
-      m, "TrajectorySource", doc.TrajectorySource.doc)
-      .def(py::init<const trajectories::Trajectory<double>&, int, bool>(),
-          py::arg("trajectory"), py::arg("output_derivative_order") = 0,
-          py::arg("zero_derivatives_beyond_limits") = true,
-          doc.TrajectorySource.ctor.doc)
-      .def("UpdateTrajectory", &TrajectorySource<double>::UpdateTrajectory,
-          py::arg("trajectory"), doc.TrajectorySource.UpdateTrajectory.doc);
-
   m.def("AddRandomInputs", &AddRandomInputs<double>,
        py::arg("sampling_interval_sec"), py::arg("builder"),
        doc.AddRandomInputs.doc)

bindings/pydrake/systems/test/primitives_test.py

@@ -57,7 +57,7 @@
     SymbolicVectorSystem, SymbolicVectorSystem_,
     TrajectoryAffineSystem, TrajectoryAffineSystem_,
     TrajectoryLinearSystem, TrajectoryLinearSystem_,
-    TrajectorySource,
+    TrajectorySource, TrajectorySource_,
     VectorLog, VectorLogSink, VectorLogSink_,
     WrapToSystem, WrapToSystem_,
     ZeroOrderHold, ZeroOrderHold_,
@@ -110,6 +110,7 @@ def test_instantiations(self):
                                    supports_symbolic=False)
         self._check_instantiations(TrajectoryLinearSystem_,
                                    supports_symbolic=False)
+        self._check_instantiations(TrajectorySource_)
         self._check_instantiations(VectorLogSink_)
         self._check_instantiations(WrapToSystem_)
         self._check_instantiations(ZeroOrderHold_)

systems/primitives/BUILD.bazel

@@ -321,6 +321,7 @@ drake_cc_library(
     hdrs = ["trajectory_source.h"],
     deps = [
         "//common/trajectories:trajectory",
+        "//math:gradient",
         "//systems/framework",
     ],
 )
@@ -673,8 +674,11 @@ drake_cc_googletest(
     deps = [
         ":trajectory_source",
         "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
+        "//common/trajectories:bezier_curve",
         "//common/trajectories:piecewise_polynomial",
         "//systems/framework",
+        "//systems/framework/test_utilities:scalar_conversion",
     ],
 )
 

systems/primitives/test/trajectory_source_test.cc

@@ -5,10 +5,13 @@
 #include <gtest/gtest.h>
 
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
+#include "drake/common/test_utilities/expect_throws_message.h"
+#include "drake/common/trajectories/bezier_curve.h"
 #include "drake/common/trajectories/piecewise_polynomial.h"
 #include "drake/systems/framework/basic_vector.h"
 #include "drake/systems/framework/context.h"
 #include "drake/systems/framework/fixed_input_port_value.h"
+#include "drake/systems/framework/test_utilities/scalar_conversion.h"
 
 using Eigen::Matrix;
 using Eigen::MatrixXd;
@@ -17,6 +20,7 @@ using std::make_unique;
 namespace drake {
 namespace systems {
 
+using symbolic::Expression;
 using trajectories::PiecewisePolynomial;
 
 namespace {
@@ -87,6 +91,81 @@ TEST_F(TrajectorySourceTest, ConstantVectorSourceIsStateless) {
   EXPECT_EQ(0, context_->num_continuous_states());
 }
 
+template <typename T>
+void TestScalar(bool zero_derivatives_beyond_limits) {
+  auto pp = PiecewisePolynomial<T>::ZeroOrderHold(Vector3<T>{0, 1, 2},
+                                                  RowVector3<T>{1.2, 3, 1.5});
+  TrajectorySource<T> source(pp, 1, zero_derivatives_beyond_limits);
+  auto context = source.CreateDefaultContext();
+  context->SetTime(0.5);
+  EXPECT_NEAR(ExtractDoubleOrThrow(source.get_output_port().Eval(*context))[0],
+              1.2, 1e-14);
+}
+
+GTEST_TEST(AdditionalTrajectorySourceTests, Scalars) {
+  for (bool zero_derivatives_beyond_limits : {true, false}) {
+    TestScalar<double>(zero_derivatives_beyond_limits);
+    TestScalar<AutoDiffXd>(zero_derivatives_beyond_limits);
+    TestScalar<symbolic::Expression>(zero_derivatives_beyond_limits);
+  }
+}
+
+GTEST_TEST(AdditionalTrajectorySourceTests, ScalarConversion) {
+  auto pp = PiecewisePolynomial<double>::ZeroOrderHold(
+      Eigen::Vector3d{0, 1, 2}, Eigen::RowVector3d{1.2, 3, 1.5});
+  for (bool zero_derivatives_beyond_limits : {true, false}) {
+    // AutoDiffXd.
+    TrajectorySource<double> source(pp, 1, zero_derivatives_beyond_limits);
+    EXPECT_TRUE(is_autodiffxd_convertible(source));
+    auto source_ad = System<double>::ToAutoDiffXd(source);
+    auto context_ad = source_ad->CreateDefaultContext();
+    context_ad->SetTime(0.5);
+    EXPECT_NEAR(
+        ExtractDoubleOrThrow(source_ad->get_output_port().Eval(*context_ad))[0],
+        1.2, 1e-14);
+
+    // Taking the gradient w.r.t to time throws.
+    context_ad->SetTime(AutoDiffXd(0.5, Eigen::Vector2d(1, 2)));
+    DRAKE_EXPECT_THROWS_MESSAGE(source_ad->get_output_port().Eval(*context_ad),
+                                ".*UpdateTrajectory.*");
+
+    // UpdateTrajectory can resolve it.
+    auto pp_ad = PiecewisePolynomial<AutoDiffXd>::ZeroOrderHold(
+        Vector3<AutoDiffXd>{0, 1, 2}, RowVector3<AutoDiffXd>{1.2, 3, 1.5});
+    source_ad->UpdateTrajectory(pp_ad);
+    EXPECT_NEAR(
+        ExtractDoubleOrThrow(source_ad->get_output_port().Eval(*context_ad))[0],
+        1.2, 1e-14);
+
+    // Symbolic.
+    EXPECT_TRUE(is_symbolic_convertible(source));
+    auto source_sym = System<double>::ToSymbolic(source);
+    auto context_sym = source_sym->CreateDefaultContext();
+    context_sym->SetTime(0.5);
+    EXPECT_NEAR(ExtractDoubleOrThrow(
+                    source_sym->get_output_port().Eval(*context_sym))[0],
+                1.2, 1e-14);
+
+    // When time is symbolic, the output port eval throws.
+    symbolic::Variable t("t");
+    context_sym->SetTime(t);
+    DRAKE_EXPECT_THROWS_MESSAGE(
+        source_sym->get_output_port().Eval(*context_sym),
+        ".*environment does not have an entry for the variable t[\\s\\S]*");
+
+    // UpdateTrajectory can resolve it. (We use BezierCurve here because
+    // PiecewiseTrajectory doesn't currently support non-double Expression for
+    // time).
+    trajectories::BezierCurve<Expression> curve_sym(
+        0.0, 2.0, RowVector2<Expression>{1.2, 3});
+    source_sym->UpdateTrajectory(curve_sym);
+    Expression expected = (1.2 * (1.0 - (min(2.0, max(0.0, t)) / 2.0)) +
+                           3.0 * (min(2.0, max(0.0, t)) / 2.0));
+    EXPECT_TRUE(
+        source_sym->get_output_port().Eval(*context_sym)[0].EqualTo(expected));
+  }
+}
+
 }  // namespace
 }  // namespace systems
 }  // namespace drake

systems/primitives/trajectory_source.cc

@@ -1,6 +1,10 @@
 #include "drake/systems/primitives/trajectory_source.h"
 
+#include <limits>
+
 #include "drake/common/drake_assert.h"
+#include "drake/common/ssize.h"
+#include "drake/math/autodiff_gradient.h"
 
 namespace drake {
 namespace systems {
@@ -11,8 +15,9 @@ template <typename T>
 TrajectorySource<T>::TrajectorySource(const Trajectory<T>& trajectory,
                                       int output_derivative_order,
                                       bool zero_derivatives_beyond_limits)
-    : SingleOutputVectorSource<T>(trajectory.rows() *
-                                  (1 + output_derivative_order)),
+    : SingleOutputVectorSource<T>(
+          SystemTypeTag<TrajectorySource>{},
+          trajectory.rows() * (1 + output_derivative_order)),
       // Make a copy of the input trajectory.
       trajectory_(trajectory.Clone()),
       clamp_derivatives_(zero_derivatives_beyond_limits) {
@@ -29,10 +34,30 @@ TrajectorySource<T>::TrajectorySource(const Trajectory<T>& trajectory,
   }
 }
 
+template <typename T>
+template <typename U>
+TrajectorySource<T>::TrajectorySource(const TrajectorySource<U>& other)
+    : SingleOutputVectorSource<T>(
+          SystemTypeTag<TrajectorySource>{},
+          other.trajectory_->rows() * (1 + ssize(other.derivatives_))),
+      clamp_derivatives_{other.clamp_derivatives_} {
+  if constexpr (std::is_same_v<U, double>) {
+    failsafe_trajectory_ = other.trajectory_->Clone();
+    for (int i = 0; i < ssize(other.derivatives_); ++i) {
+      failsafe_derivatives_.emplace_back(other.derivatives_[i]->Clone());
+    }
+  } else {
+    throw std::runtime_error(
+        "TrajectorySource only supports scalar conversion from double.");
+  }
+}
+
 template <typename T>
 void TrajectorySource<T>::UpdateTrajectory(
     const trajectories::Trajectory<T>& trajectory) {
-  DRAKE_DEMAND(trajectory.rows() == trajectory_->rows());
+  DRAKE_DEMAND(trajectory_ != nullptr || failsafe_trajectory_ != nullptr);
+  int rows = trajectory_ ? trajectory_->rows() : failsafe_trajectory_->rows();
+  DRAKE_DEMAND(trajectory.rows() == rows);
   DRAKE_DEMAND(trajectory.cols() == 1);
 
   trajectory_ = trajectory.Clone();
@@ -42,29 +67,59 @@ void TrajectorySource<T>::UpdateTrajectory(
     else
       derivatives_[i] = derivatives_[i - 1]->MakeDerivative();
   }
+  failsafe_trajectory_ = nullptr;
+  failsafe_derivatives_.clear();
 }
 
 template <typename T>
 void TrajectorySource<T>::DoCalcVectorOutput(
     const Context<T>& context, Eigen::VectorBlock<VectorX<T>>* output) const {
-  int len = trajectory_->rows();
-  output->head(len) = trajectory_->value(context.get_time());
+  DRAKE_DEMAND(trajectory_ != nullptr || failsafe_trajectory_ != nullptr);
+  int len = trajectory_ ? trajectory_->rows() : failsafe_trajectory_->rows();
+  T time = context.get_time();
+  double maybe_time_as_double{std::numeric_limits<double>::quiet_NaN()};
+  if (trajectory_ != nullptr) {
+    output->head(len) = trajectory_->value(time);
+  } else {
+    // TODO(russt): We are missing DiscardZeroGradient for scalars, and/or
+    // a version of ExtractDoubleOrThrow which throws on non-zero gradients.
+    if constexpr (std::is_same_v<T, AutoDiffXd>) {
+      if (time.derivatives().size() != 0 && !time.derivatives().isZero()) {
+        throw std::runtime_error(
+            "Cannot take gradients w.r.t. time with this TrajectorySource. You "
+            "must call UpdateTrajectory() to provide a "
+            "Trajectory<AutoDiffXd>.");
+      }
+    }
+    maybe_time_as_double = ExtractDoubleOrThrow(context.get_time());
+    output->head(len) = failsafe_trajectory_->value(maybe_time_as_double);
+  }
 
-  double time = context.get_time();
-  bool set_zero = clamp_derivatives_ && (time > trajectory_->end_time() ||
-                                         time < trajectory_->start_time());
+  T start_time = trajectory_ ? trajectory_->start_time()
+                             : failsafe_trajectory_->start_time();
+  T end_time =
+      trajectory_ ? trajectory_->end_time() : failsafe_trajectory_->end_time();
+  boolean<T> set_zero =
+      boolean<T>(clamp_derivatives_) && (time > end_time || time < start_time);
 
-  for (size_t i = 0; i < derivatives_.size(); ++i) {
-    if (set_zero) {
-      output->segment(len * (i + 1), len).setZero();
-    } else {
-      output->segment(len * (i + 1), len) =
-          derivatives_[i]->value(context.get_time());
+  DRAKE_DEMAND(derivatives_.size() == 0 || failsafe_derivatives_.size() == 0);
+  for (int i = 0; i < ssize(derivatives_); ++i) {
+    VectorX<T> value = derivatives_[i]->value(context.get_time());
+    for (int j = 0; j < len; ++j) {
+      (*output)[len * (i + 1) + j] = if_then_else(set_zero, T(0), value[j]);
+    }
+  }
+  for (int i = 0; i < ssize(failsafe_derivatives_); ++i) {
+    DRAKE_DEMAND(!std::isnan(maybe_time_as_double));
+    VectorX<T> value = failsafe_derivatives_[i]->value(maybe_time_as_double);
+    for (int j = 0; j < len; ++j) {
+      (*output)[len * (i + 1) + j] = if_then_else(set_zero, T(0), value[j]);
     }
   }
 }
 
-template class TrajectorySource<double>;
-
 }  // namespace systems
 }  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::systems::TrajectorySource)

systems/primitives/trajectory_source.h

@@ -26,8 +26,12 @@ namespace systems {
 /// - y0
 /// @endsystem
 ///
-/// @tparam_double_only
-/// @ingroup primitive_systems
+/// Note: Scalar conversion is supported, but the stored Trajectory is not
+/// automatically scalar converted. You must call UpdateTrajectory() with an
+/// updated Trajectory<T> in order to fully-enable scalar-type support on the
+/// trajectory parameters/values.
+///
+/// @tparam_default_scalar @ingroup primitive_systems
 template <typename T>
 class TrajectorySource final : public SingleOutputVectorSource<T> {
  public:
@@ -37,20 +41,29 @@ class TrajectorySource final : public SingleOutputVectorSource<T> {
   /// @param output_derivative_order The number of times to take the derivative.
   /// Must be greater than or equal to zero.
   /// @param zero_derivatives_beyond_limits All derivatives will be zero before
-  /// the start time or after the end time of @p trajectory.
+  /// the start time or after the end time of @p trajectory. However, this
+  /// clamping is ignored for T=Expression.
   /// @pre The value of `trajectory` is a column vector. More precisely,
   /// trajectory.cols() == 1.
   explicit TrajectorySource(const trajectories::Trajectory<T>& trajectory,
                             int output_derivative_order = 0,
                             bool zero_derivatives_beyond_limits = true);
 
+  // Scalar-converting copy constructor.  See @ref system_scalar_conversion.
+  template <typename U>
+  explicit TrajectorySource(const TrajectorySource<U>& other);
+
   ~TrajectorySource() final = default;
 
   /// Updates the stored trajectory. @p trajectory must have the same number of
   /// rows as the trajectory passed to the constructor.
   void UpdateTrajectory(const trajectories::Trajectory<T>& trajectory);
 
  private:
+  // TrajectorySource of one scalar type is friends with all other scalar types.
+  template <typename>
+  friend class TrajectorySource;
+
   // Outputs a vector of values evaluated at the context time of the trajectory
   // and up to its Nth derivatives, where the trajectory and N are passed to
   // the constructor. The size of the vector is:
@@ -59,10 +72,23 @@ class TrajectorySource final : public SingleOutputVectorSource<T> {
   void DoCalcVectorOutput(const Context<T>& context,
                           Eigen::VectorBlock<VectorX<T>>* output) const final;
 
-  std::unique_ptr<trajectories::Trajectory<T>> trajectory_;
+  std::unique_ptr<trajectories::Trajectory<T>> trajectory_{};
   const bool clamp_derivatives_;
-  std::vector<std::unique_ptr<trajectories::Trajectory<T>>> derivatives_;
+  std::vector<std::unique_ptr<trajectories::Trajectory<T>>> derivatives_{};
+
+  // These are used when trajectory_ and derivatives_ are nullptr, e.g.,
+  // after a scalar conversion. Scalar converting double => non-double ends
+  // up cloning the T=double trajectory here, until UpdateTrajectory is used
+  // to replace it.
+  // TODO(russt): Remove these if/when we support automatic scalar conversion
+  // of Trajectory classes.
+  std::unique_ptr<trajectories::Trajectory<double>> failsafe_trajectory_{};
+  std::vector<std::unique_ptr<trajectories::Trajectory<double>>>
+      failsafe_derivatives_{};
 };
 
 }  // namespace systems
 }  // namespace drake
+
+DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::systems::TrajectorySource)