From ecc99b7e5700425ca88ee4126d5e38fbe3466080 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Tue, 8 Aug 2023 18:21:48 -0400 Subject: [PATCH] Support default scalars for TrajectorySource --- bindings/pydrake/systems/primitives_py.cc | 18 ++-- .../pydrake/systems/test/primitives_test.py | 3 +- systems/primitives/BUILD.bazel | 4 + .../primitives/test/trajectory_source_test.cc | 79 +++++++++++++++++ systems/primitives/trajectory_source.cc | 87 +++++++++++++++---- systems/primitives/trajectory_source.h | 36 ++++++-- 6 files changed, 196 insertions(+), 31 deletions(-) diff --git a/bindings/pydrake/systems/primitives_py.cc b/bindings/pydrake/systems/primitives_py.cc index 7df53ffb45f4..b98d83bb0db9 100644 --- a/bindings/pydrake/systems/primitives_py.cc +++ b/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::period, doc.ZeroOrderHold.period.doc) .def("offset", &ZeroOrderHold::offset, doc.ZeroOrderHold.offset.doc); + + DefineTemplateClassWithDefault, LeafSystem>( + m, "TrajectorySource", GetPyParam(), doc.TrajectorySource.doc) + .def(py::init&, 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::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_, LeafSystem>( - m, "TrajectorySource", doc.TrajectorySource.doc) - .def(py::init&, 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::UpdateTrajectory, - py::arg("trajectory"), doc.TrajectorySource.UpdateTrajectory.doc); - m.def("AddRandomInputs", &AddRandomInputs, py::arg("sampling_interval_sec"), py::arg("builder"), doc.AddRandomInputs.doc) diff --git a/bindings/pydrake/systems/test/primitives_test.py b/bindings/pydrake/systems/test/primitives_test.py index 30cb03ae7606..ed3edcf5bd9b 100644 --- a/bindings/pydrake/systems/test/primitives_test.py +++ b/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_) diff --git a/systems/primitives/BUILD.bazel b/systems/primitives/BUILD.bazel index 2e1aca5a390b..8bbfe211b15c 100644 --- a/systems/primitives/BUILD.bazel +++ b/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", ], ) diff --git a/systems/primitives/test/trajectory_source_test.cc b/systems/primitives/test/trajectory_source_test.cc index 84129b26a041..920418701de1 100644 --- a/systems/primitives/test/trajectory_source_test.cc +++ b/systems/primitives/test/trajectory_source_test.cc @@ -5,10 +5,13 @@ #include #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 +void TestScalar(bool zero_derivatives_beyond_limits) { + auto pp = PiecewisePolynomial::ZeroOrderHold(Vector3{0, 1, 2}, + RowVector3{1.2, 3, 1.5}); + TrajectorySource 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(zero_derivatives_beyond_limits); + TestScalar(zero_derivatives_beyond_limits); + TestScalar(zero_derivatives_beyond_limits); + } +} + +GTEST_TEST(AdditionalTrajectorySourceTests, ScalarConversion) { + auto pp = PiecewisePolynomial::ZeroOrderHold( + Eigen::Vector3d{0, 1, 2}, Eigen::RowVector3d{1.2, 3, 1.5}); + for (bool zero_derivatives_beyond_limits : {true, false}) { + // AutoDiffXd. + TrajectorySource source(pp, 1, zero_derivatives_beyond_limits); + EXPECT_TRUE(is_autodiffxd_convertible(source)); + auto source_ad = System::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::ZeroOrderHold( + Vector3{0, 1, 2}, RowVector3{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::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 curve_sym( + 0.0, 2.0, RowVector2{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 diff --git a/systems/primitives/trajectory_source.cc b/systems/primitives/trajectory_source.cc index 37fa10a30342..0ecfb2457c51 100644 --- a/systems/primitives/trajectory_source.cc +++ b/systems/primitives/trajectory_source.cc @@ -1,6 +1,10 @@ #include "drake/systems/primitives/trajectory_source.h" +#include + #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 TrajectorySource::TrajectorySource(const Trajectory& trajectory, int output_derivative_order, bool zero_derivatives_beyond_limits) - : SingleOutputVectorSource(trajectory.rows() * - (1 + output_derivative_order)), + : SingleOutputVectorSource( + SystemTypeTag{}, + 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::TrajectorySource(const Trajectory& trajectory, } } +template +template +TrajectorySource::TrajectorySource(const TrajectorySource& other) + : SingleOutputVectorSource( + SystemTypeTag{}, + other.trajectory_->rows() * (1 + ssize(other.derivatives_))), + clamp_derivatives_{other.clamp_derivatives_} { + if constexpr (std::is_same_v) { + 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 void TrajectorySource::UpdateTrajectory( const trajectories::Trajectory& 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::UpdateTrajectory( else derivatives_[i] = derivatives_[i - 1]->MakeDerivative(); } + failsafe_trajectory_ = nullptr; + failsafe_derivatives_.clear(); } template void TrajectorySource::DoCalcVectorOutput( const Context& context, Eigen::VectorBlock>* 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::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) { + 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."); + } + } + 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 set_zero = + boolean(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 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 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; - } // namespace systems } // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::systems::TrajectorySource) diff --git a/systems/primitives/trajectory_source.h b/systems/primitives/trajectory_source.h index 8ab24f739871..5f812b3d30e0 100644 --- a/systems/primitives/trajectory_source.h +++ b/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 in order to fully-enable scalar-type support on the +/// trajectory parameters/values. +/// +/// @tparam_default_scalar @ingroup primitive_systems template class TrajectorySource final : public SingleOutputVectorSource { public: @@ -37,13 +41,18 @@ class TrajectorySource final : public SingleOutputVectorSource { /// @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& trajectory, int output_derivative_order = 0, bool zero_derivatives_beyond_limits = true); + // Scalar-converting copy constructor. See @ref system_scalar_conversion. + template + explicit TrajectorySource(const TrajectorySource& other); + ~TrajectorySource() final = default; /// Updates the stored trajectory. @p trajectory must have the same number of @@ -51,6 +60,10 @@ class TrajectorySource final : public SingleOutputVectorSource { void UpdateTrajectory(const trajectories::Trajectory& trajectory); private: + // TrajectorySource of one scalar type is friends with all other scalar types. + template + 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 { void DoCalcVectorOutput(const Context& context, Eigen::VectorBlock>* output) const final; - std::unique_ptr> trajectory_; + std::unique_ptr> trajectory_{}; const bool clamp_derivatives_; - std::vector>> derivatives_; + std::vector>> 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> failsafe_trajectory_{}; + std::vector>> + failsafe_derivatives_{}; }; } // namespace systems } // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::systems::TrajectorySource)