From d97c505dd285ffb6035c64d6f020eb60bf9f2810 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Sun, 5 Jan 2025 16:47:09 -0800 Subject: [PATCH] [trajectories] Deprecate overriding public virtual functions (#22395) Subclasses must switch to using NVI overrides, instead. In most cases where subclass functions moved from public to private we also relocate the code in the cc file to stay in sync with header file order. However, the piecewise_polynomial.cc file is so disorganized that we declare bankruptcy and don't move anything around in there. --- bindings/pydrake/BUILD.bazel | 1 + bindings/pydrake/test/trajectories_test.py | 80 ++++++++++++- bindings/pydrake/trajectories_py.cc | 106 +++++++++++------- common/trajectories/BUILD.bazel | 2 + common/trajectories/bezier_curve.cc | 74 ++++++------ common/trajectories/bezier_curve.h | 38 +++---- common/trajectories/bspline_trajectory.cc | 25 +++-- common/trajectories/bspline_trajectory.h | 34 +++--- common/trajectories/composite_trajectory.cc | 44 ++++---- common/trajectories/composite_trajectory.h | 18 +-- common/trajectories/derivative_trajectory.cc | 34 +++--- common/trajectories/derivative_trajectory.h | 15 +-- .../trajectories/discrete_time_trajectory.cc | 12 +- .../trajectories/discrete_time_trajectory.h | 38 +++++-- .../exponential_plus_piecewise_polynomial.cc | 52 ++++----- .../exponential_plus_piecewise_polynomial.h | 23 ++-- .../function_handle_trajectory.cc | 10 +- .../trajectories/function_handle_trajectory.h | 20 ++-- .../path_parameterized_trajectory.cc | 6 +- .../path_parameterized_trajectory.h | 40 +++---- ...piecewise_constant_curvature_trajectory.cc | 22 ++-- .../piecewise_constant_curvature_trajectory.h | 22 ++-- common/trajectories/piecewise_polynomial.cc | 6 +- common/trajectories/piecewise_polynomial.h | 36 +++--- common/trajectories/piecewise_pose.cc | 12 +- common/trajectories/piecewise_pose.h | 27 ++--- common/trajectories/piecewise_quaternion.cc | 12 +- common/trajectories/piecewise_quaternion.h | 29 ++--- common/trajectories/piecewise_trajectory.cc | 20 ++-- common/trajectories/piecewise_trajectory.h | 10 +- common/trajectories/stacked_trajectory.cc | 60 +++++----- common/trajectories/stacked_trajectory.h | 14 +-- common/trajectories/trajectory.cc | 57 ++++++++++ common/trajectories/trajectory.h | 56 ++++++++- 34 files changed, 628 insertions(+), 427 deletions(-) diff --git a/bindings/pydrake/BUILD.bazel b/bindings/pydrake/BUILD.bazel index d583e65a8af6..557e7cb3c57c 100644 --- a/bindings/pydrake/BUILD.bazel +++ b/bindings/pydrake/BUILD.bazel @@ -373,6 +373,7 @@ drake_py_unittest( name = "trajectories_test", deps = [ ":trajectories_py", + "//bindings/pydrake/common/test_utilities:deprecation_py", "//bindings/pydrake/common/test_utilities:numpy_compare_py", "//bindings/pydrake/common/test_utilities:pickle_compare_py", "//bindings/pydrake/common/test_utilities:scipy_stub_py", diff --git a/bindings/pydrake/test/trajectories_test.py b/bindings/pydrake/test/trajectories_test.py index af8e190622d6..67addec942e4 100644 --- a/bindings/pydrake/test/trajectories_test.py +++ b/bindings/pydrake/test/trajectories_test.py @@ -8,6 +8,7 @@ from pydrake.common import ToleranceType from pydrake.common.eigen_geometry import AngleAxis_, Quaternion_ from pydrake.common.test_utilities import numpy_compare +from pydrake.common.test_utilities.deprecation import catch_drake_warnings from pydrake.common.test_utilities.pickle_compare import assert_pickle from pydrake.common.value import AbstractValue from pydrake.common.yaml import yaml_load_typed @@ -31,26 +32,71 @@ from pydrake.symbolic import Variable, Expression -# Custom trajectory class used to test Trajectory subclassing in python. +# Custom trajectory class used to test Trajectory subclassing in Python. class CustomTrajectory(Trajectory): def __init__(self): Trajectory.__init__(self) + # TODO(jwnimmer-tri) Should be __deepcopy__ not Clone. def Clone(self): return CustomTrajectory() + def do_value(self, t): + return np.array([[t + 1.0, t + 2.0]]) + + def do_has_derivative(self): + return True + + def DoEvalDerivative(self, t, derivative_order): + if derivative_order >= 2: + return np.zeros((1, 2)) + elif derivative_order == 1: + return np.ones((1, 2)) + elif derivative_order == 0: + return self.value(t) + + def DoMakeDerivative(self, derivative_order): + return DerivativeTrajectory_[float](self, derivative_order) + + def do_rows(self): + return 1 + + def do_cols(self): + return 2 + + def do_start_time(self): + return 3.0 + + def do_end_time(self): + return 4.0 + + +# Legacy custom trajectory class used to test Trajectory subclassing in Python. +# This uses the old spellings for how to override the NVI functions. +class LegacyCustomTrajectory(Trajectory): + def __init__(self): + Trajectory.__init__(self) + + def Clone(self): + return LegacyCustomTrajectory() + + # This spelling of the virtual override is deprecated. def rows(self): return 1 + # This spelling of the virtual override is deprecated. def cols(self): return 2 + # This spelling of the virtual override is deprecated. def start_time(self): return 3.0 + # This spelling of the virtual override is deprecated. def end_time(self): return 4.0 + # This spelling of the virtual override is deprecated. def value(self, t): return np.array([[t + 1.0, t + 2.0]]) @@ -98,6 +144,38 @@ def test_custom_trajectory(self): numpy_compare.assert_float_equal( deriv.value(t=2.3), np.ones((1, 2))) + def test_legacy_custom_trajectory(self): + trajectory = LegacyCustomTrajectory() + self.assertEqual(trajectory.rows(), 1) + self.assertEqual(trajectory.cols(), 2) + self.assertEqual(trajectory.start_time(), 3.0) + self.assertEqual(trajectory.end_time(), 4.0) + self.assertTrue(trajectory.has_derivative()) + numpy_compare.assert_float_equal(trajectory.value(t=1.5), + np.array([[2.5, 3.5]])) + numpy_compare.assert_float_equal( + trajectory.EvalDerivative(t=2.3, derivative_order=1), + np.ones((1, 2))) + numpy_compare.assert_float_equal( + trajectory.EvalDerivative(t=2.3, derivative_order=2), + np.zeros((1, 2))) + + # Use StackedTrajectory to call the deprecated overrides from C++ so + # that we trigger the deprecation warnings. + stacked = StackedTrajectory_[float]() + with catch_drake_warnings(): + # The C++ code calls rows() and cols() -- both of which cause + # deprecation warnings with the legacy overrides -- but the total + # number of calls varies between Debug and Release, so we don't + # check the exact tally here. + stacked.Append(trajectory) + with catch_drake_warnings(expected_count=1): + stacked.value(t=1.5) + with catch_drake_warnings(expected_count=1): + self.assertEqual(stacked.start_time(), 3.0) + with catch_drake_warnings(expected_count=1): + self.assertEqual(stacked.end_time(), 4.0) + @numpy_compare.check_all_types def test_bezier_curve(self, T): curve = BezierCurve_[T]() diff --git a/bindings/pydrake/trajectories_py.cc b/bindings/pydrake/trajectories_py.cc index 46c72e28ef73..0a142bd7a33c 100644 --- a/bindings/pydrake/trajectories_py.cc +++ b/bindings/pydrake/trajectories_py.cc @@ -1,9 +1,11 @@ #include "pybind11/eval.h" #include "drake/bindings/pydrake/common/default_scalars_pybind.h" +#include "drake/bindings/pydrake/common/deprecation_pybind.h" #include "drake/bindings/pydrake/documentation_pybind.h" #include "drake/bindings/pydrake/polynomial_types_pybind.h" #include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/common/nice_type_name.h" #include "drake/common/polynomial.h" #include "drake/common/trajectories/bezier_curve.h" #include "drake/common/trajectories/bspline_trajectory.h" @@ -158,15 +160,13 @@ struct Impl { TrajectoryPublic() = default; // Virtual methods, for explicit bindings. - using Base::Clone; - using Base::cols; + using Base::do_cols; + using Base::do_end_time; using Base::do_has_derivative; - using Base::DoEvalDerivative; - using Base::DoMakeDerivative; - using Base::end_time; - using Base::rows; - using Base::start_time; - using Base::value; + using Base::do_rows; + using Base::do_start_time; + using Base::do_value; + using Base::DoClone; }; class PyTrajectory : public py::wrapper { @@ -174,35 +174,40 @@ struct Impl { using Base = py::wrapper; using Base::Base; - // Trampoline virtual methods. - MatrixX value(const T& t) const override { - PYBIND11_OVERLOAD_PURE(MatrixX, Trajectory, value, t); - } - - T start_time() const override { - PYBIND11_OVERLOAD_PURE(T, Trajectory, start_time); + void WarnDeprecatedOverride(std::string_view func_name) const { + WarnDeprecated( + fmt::format( + "Support for overriding {}.{} as a virtual function is " + "deprecated. Subclasses should override the do_{} virtual " + "function, instead.", + NiceTypeName::Get(*this), func_name, func_name), + "2025-08-01"); } - T end_time() const override { - PYBIND11_OVERLOAD_PURE(T, Trajectory, end_time); - } + // Trampoline virtual methods. - Eigen::Index rows() const override { - PYBIND11_OVERLOAD_PURE(Eigen::Index, Trajectory, rows); + std::unique_ptr> DoClone() const final { + // TODO(jwnimmer-tri) Rewrite cloning to use __deepcopy__ in lieu of + // Clone (or DoClone). + PYBIND11_OVERLOAD_PURE( + std::unique_ptr>, Trajectory, Clone); } - Eigen::Index cols() const override { - PYBIND11_OVERLOAD_PURE(Eigen::Index, Trajectory, cols); + MatrixX do_value(const T& t) const final { + PYBIND11_OVERLOAD_INT(MatrixX, Trajectory, "do_value", t); + WarnDeprecatedOverride("value"); + PYBIND11_OVERLOAD_INT(MatrixX, Trajectory, "value", t); + // If the macro did not return, use default functionality. + return Base::do_value(t); } - bool do_has_derivative() const override { + bool do_has_derivative() const final { PYBIND11_OVERLOAD_INT(bool, Trajectory, "do_has_derivative"); // If the macro did not return, use default functionality. return Base::do_has_derivative(); } - MatrixX DoEvalDerivative( - const T& t, int derivative_order) const override { + MatrixX DoEvalDerivative(const T& t, int derivative_order) const final { PYBIND11_OVERLOAD_INT( MatrixX, Trajectory, "DoEvalDerivative", t, derivative_order); // If the macro did not return, use default functionality. @@ -210,16 +215,43 @@ struct Impl { } std::unique_ptr> DoMakeDerivative( - int derivative_order) const override { + int derivative_order) const final { PYBIND11_OVERLOAD_INT(std::unique_ptr>, Trajectory, "DoMakeDerivative", derivative_order); // If the macro did not return, use default functionality. return Base::DoMakeDerivative(derivative_order); } - std::unique_ptr> Clone() const override { - PYBIND11_OVERLOAD_PURE( - std::unique_ptr>, Trajectory, Clone); + T do_start_time() const final { + PYBIND11_OVERLOAD_INT(T, Trajectory, "do_start_time"); + WarnDeprecatedOverride("start_time"); + PYBIND11_OVERLOAD_INT(T, Trajectory, "start_time"); + // If the macro did not return, use default functionality. + return Base::do_start_time(); + } + + T do_end_time() const final { + PYBIND11_OVERLOAD_INT(T, Trajectory, "do_end_time"); + WarnDeprecatedOverride("end_time"); + PYBIND11_OVERLOAD_INT(T, Trajectory, "end_time"); + // If the macro did not return, use default functionality. + return Base::do_end_time(); + } + + Eigen::Index do_rows() const final { + PYBIND11_OVERLOAD_INT(Eigen::Index, Trajectory, "do_rows"); + WarnDeprecatedOverride("rows"); + PYBIND11_OVERLOAD_INT(Eigen::Index, Trajectory, "rows"); + // If the macro did not return, use default functionality. + return Base::do_rows(); + } + + Eigen::Index do_cols() const final { + PYBIND11_OVERLOAD_INT(Eigen::Index, Trajectory, "do_cols"); + WarnDeprecatedOverride("cols"); + PYBIND11_OVERLOAD_INT(Eigen::Index, Trajectory, "cols"); + // If the macro did not return, use default functionality. + return Base::do_cols(); } }; @@ -252,6 +284,11 @@ struct Impl { .def("end_time", &Class::end_time, cls_doc.end_time.doc) .def("rows", &Class::rows, cls_doc.rows.doc) .def("cols", &Class::cols, cls_doc.cols.doc); + // This binds Clone() and __copy__ and __deepcopy__. For final subclasses + // of Trajectory that offer a public copy constructor, in their bindings + // we also call DefCopyAndDeepCopy to override __copy__ and __deepcopy__ + // with a more efficient implementation. + DefClone(&cls); } { @@ -298,7 +335,6 @@ struct Impl { py::arg("basis"), py::arg("control_points"), cls_doc.ctor.doc) .def(py::init, std::vector>>(), py::arg("basis"), py::arg("control_points"), cls_doc.ctor.doc) - .def("Clone", &Class::Clone, cls_doc.Clone.doc) .def("num_control_points", &Class::num_control_points, cls_doc.num_control_points.doc) .def("control_points", &Class::control_points, @@ -330,8 +366,7 @@ struct Impl { m, "DerivativeTrajectory", param, cls_doc.doc); cls // BR .def(py::init&, int>(), py::arg("nominal"), - py::arg("derivative_order") = 1, cls_doc.ctor.doc) - .def("Clone", &Class::Clone, cls_doc.Clone.doc); + py::arg("derivative_order") = 1, cls_doc.ctor.doc); DefCopyAndDeepCopy(&cls); } @@ -348,8 +383,7 @@ struct Impl { py::arg("end_time") = std::numeric_limits::infinity(), cls_doc.ctor.doc) .def("set_derivative", &Class::set_derivative, py::arg("func"), - cls_doc.set_derivative.doc) - .def("Clone", &Class::Clone, cls_doc.Clone.doc); + cls_doc.set_derivative.doc); DefCopyAndDeepCopy(&cls); } @@ -361,7 +395,6 @@ struct Impl { cls // BR .def(py::init&, const Trajectory&>(), py::arg("path"), py::arg("time_scaling"), cls_doc.ctor.doc) - .def("Clone", &Class::Clone, cls_doc.Clone.doc) .def("path", &Class::path, py_rvp::reference_internal, cls_doc.path.doc) .def("time_scaling", &Class::time_scaling, py_rvp::reference_internal, @@ -413,7 +446,6 @@ struct Impl { .def(py::init> const&, std::vector const&>(), cls_doc.ctor.doc_2args_polynomials_breaks) - .def("Clone", &Class::Clone, cls_doc.Clone.doc) .def_static( "ZeroOrderHold", // This serves the same purpose as the C++ @@ -632,7 +664,6 @@ struct Impl { }), py::arg("K"), py::arg("A"), py::arg("alpha"), py::arg("piecewise_polynomial_part"), cls_doc.ctor.doc) - .def("Clone", &Class::Clone, cls_doc.Clone.doc) .def("shiftRight", &Class::shiftRight, py::arg("offset"), cls_doc.shiftRight.doc); DefCopyAndDeepCopy(&cls); @@ -723,7 +754,6 @@ struct Impl { m, "StackedTrajectory", param, cls_doc.doc); cls // BR .def(py::init(), py::arg("rowwise") = true, cls_doc.ctor.doc) - .def("Clone", &Class::Clone, cls_doc.Clone.doc) .def("Append", py::overload_cast&>(&Class::Append), /* N.B. We choose to omit any py::arg name here. */ diff --git a/common/trajectories/BUILD.bazel b/common/trajectories/BUILD.bazel index f4fed5796e05..1d35a2c5056c 100644 --- a/common/trajectories/BUILD.bazel +++ b/common/trajectories/BUILD.bazel @@ -214,6 +214,8 @@ drake_cc_library( "//common:default_scalars", "//common:essential", "//common:unused", + # Remove with deprecation 2025-08-01. + "//common:nice_type_name", ], ) diff --git a/common/trajectories/bezier_curve.cc b/common/trajectories/bezier_curve.cc index eb5512e5f2d4..77d5deeb7aee 100644 --- a/common/trajectories/bezier_curve.cc +++ b/common/trajectories/bezier_curve.cc @@ -35,18 +35,6 @@ T BezierCurve::BernsteinBasis(int i, const T& time, return coeff * pow(s, i) * pow(1 - s, n - i); } -template -std::unique_ptr> BezierCurve::Clone() const { - return std::make_unique>(start_time_, end_time_, - control_points_); -} - -template -MatrixX BezierCurve::value(const T& time) const { - using std::clamp; - return EvaluateT(clamp(time, T{start_time_}, T{end_time_})); -} - template VectorX BezierCurve::GetExpression( symbolic::Variable time) const { @@ -80,7 +68,7 @@ VectorX BezierCurve::GetExpression( template void BezierCurve::ElevateOrder() { if (order() < 0) { - control_points_ = MatrixX::Zero(rows(), cols()); + control_points_ = MatrixX::Zero(this->rows(), this->cols()); return; } // https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/Bezier/bezier-elev.html @@ -141,18 +129,15 @@ SparseMatrix BezierCurve::AsLinearInControlPoints( } template -MatrixX BezierCurve::CalcDerivativePoints(int derivative_order) const { - DRAKE_DEMAND(derivative_order <= order()); - int n = order(); - MatrixX points = - (control_points_.rightCols(order()) - control_points_.leftCols(order())) * - order() / (end_time_ - start_time_); - for (int i = 1; i < derivative_order; ++i) { - n -= 1; - points = (points.rightCols(n) - points.leftCols(n)).eval() * n / - (end_time_ - start_time_); - } - return points; +std::unique_ptr> BezierCurve::DoClone() const { + return std::make_unique>(start_time_, end_time_, + control_points_); +} + +template +MatrixX BezierCurve::do_value(const T& time) const { + using std::clamp; + return EvaluateT(clamp(time, T{start_time_}, T{end_time_})); } template @@ -163,29 +148,20 @@ MatrixX BezierCurve::DoEvalDerivative(const T& time, return this->value(time); } if (derivative_order > order()) { - return VectorX::Zero(rows()); + return VectorX::Zero(this->rows()); } MatrixX points = CalcDerivativePoints(derivative_order); using std::clamp; const T ctime = clamp(time, T{start_time_}, T{end_time_}); - MatrixX v = VectorX::Zero(rows()); + MatrixX v = VectorX::Zero(this->rows()); for (int i = 0; i < points.cols(); ++i) { v += BernsteinBasis(i, ctime, order() - derivative_order) * points.col(i); } return v; } -template -VectorX BezierCurve::EvaluateT(const T& time) const { - VectorX v = VectorX::Zero(rows()); - for (int i = 0; i < control_points_.cols(); ++i) { - v += BernsteinBasis(i, time) * control_points_.col(i); - } - return v; -} - template std::unique_ptr> BezierCurve::DoMakeDerivative( int derivative_order) const { @@ -196,13 +172,37 @@ std::unique_ptr> BezierCurve::DoMakeDerivative( if (derivative_order > order()) { // Then return the zero curve. return std::make_unique>(start_time_, end_time_, - VectorX::Zero(rows())); + VectorX::Zero(this->rows())); } return std::make_unique>( start_time_, end_time_, CalcDerivativePoints(derivative_order)); } +template +MatrixX BezierCurve::CalcDerivativePoints(int derivative_order) const { + DRAKE_DEMAND(derivative_order <= order()); + int n = order(); + MatrixX points = + (control_points_.rightCols(order()) - control_points_.leftCols(order())) * + order() / (end_time_ - start_time_); + for (int i = 1; i < derivative_order; ++i) { + n -= 1; + points = (points.rightCols(n) - points.leftCols(n)).eval() * n / + (end_time_ - start_time_); + } + return points; +} + +template +VectorX BezierCurve::EvaluateT(const T& time) const { + VectorX v = VectorX::Zero(this->rows()); + for (int i = 0; i < control_points_.cols(); ++i) { + v += BernsteinBasis(i, time) * control_points_.col(i); + } + return v; +} + DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class BezierCurve); diff --git a/common/trajectories/bezier_curve.h b/common/trajectories/bezier_curve.h index 252e414c0894..ebdce8a3bd66 100644 --- a/common/trajectories/bezier_curve.h +++ b/common/trajectories/bezier_curve.h @@ -41,7 +41,7 @@ class BezierCurve final : public trajectories::Trajectory { // TODO(russt): Add support for MatrixX control points, but only if we have a // use case for it. - ~BezierCurve() override; + ~BezierCurve() final; /** Returns the order of the curve (1 for linear, 2 for quadratic, etc.). */ int order() const { return control_points_.cols() - 1; } @@ -91,16 +91,15 @@ class BezierCurve final : public trajectories::Trajectory { Eigen::SparseMatrix AsLinearInControlPoints( int derivative_order = 1) const; - // Required methods for trajectories::Trajectory interface. - - std::unique_ptr> Clone() const override; - /** Evaluates the curve at the given time. @warning If t does not lie in the range [start_time(), end_time()], the trajectory will silently be evaluated at the closest valid value of time to `time`. For example, `value(-1)` will return `value(0)` for a trajectory defined over [0, 1]. */ - MatrixX value(const T& time) const override; + MatrixX value(const T& t) const final { + // We shadowed the base class to add documentation, not to change logic. + return Trajectory::value(t); + } /** Extracts the expanded underlying polynomial expression of this curve in terms of variable `time`. */ @@ -112,25 +111,22 @@ class BezierCurve final : public trajectories::Trajectory { control points of `this` are modified to obtain the equivalent curve. */ void ElevateOrder(); - Eigen::Index rows() const override { return control_points_.rows(); } - - Eigen::Index cols() const override { return 1; } - - T start_time() const override { return start_time_; } - - T end_time() const override { return end_time_; } - private: + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final; + bool do_has_derivative() const final { return true; } + MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; + std::unique_ptr> DoMakeDerivative( + int derivative_order) const final; + Eigen::Index do_rows() const final { return control_points_.rows(); } + Eigen::Index do_cols() const final { return 1; } + T do_start_time() const final { return start_time_; } + T do_end_time() const final { return end_time_; } + /* Calculates the control points of the derivative curve. */ MatrixX CalcDerivativePoints(int derivative_order) const; - bool do_has_derivative() const override { return true; } - - MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; - - std::unique_ptr> DoMakeDerivative( - int derivative_order) const override; - VectorX EvaluateT(const T& time) const; double start_time_{}; diff --git a/common/trajectories/bspline_trajectory.cc b/common/trajectories/bspline_trajectory.cc index defdde92c6ca..b9f5e3dced22 100644 --- a/common/trajectories/bspline_trajectory.cc +++ b/common/trajectories/bspline_trajectory.cc @@ -30,15 +30,15 @@ template BsplineTrajectory::~BsplineTrajectory() = default; template -std::unique_ptr> BsplineTrajectory::Clone() const { +std::unique_ptr> BsplineTrajectory::DoClone() const { return std::make_unique>(*this); } template -MatrixX BsplineTrajectory::value(const T& time) const { +MatrixX BsplineTrajectory::do_value(const T& time) const { using std::clamp; - return basis().EvaluateCurve(control_points(), - clamp(time, start_time(), end_time())); + return basis().EvaluateCurve( + control_points(), clamp(time, this->start_time(), this->end_time())); } template @@ -52,10 +52,10 @@ MatrixX BsplineTrajectory::DoEvalDerivative(const T& time, if (derivative_order == 0) { return this->value(time); } else if (derivative_order >= basis_.order()) { - return MatrixX::Zero(rows(), cols()); + return MatrixX::Zero(this->rows(), this->cols()); } else if (derivative_order >= 1) { using std::clamp; - T clamped_time = clamp(time, start_time(), end_time()); + T clamped_time = clamp(time, this->start_time(), this->end_time()); // For a bspline trajectory of order n, the evaluation of k th derivative // should take O(k^2) time by leveraging the sparsity of basis value. // This differs from DoMakeDerivative, which takes O(nk) time. @@ -76,7 +76,7 @@ MatrixX BsplineTrajectory::DoEvalDerivative(const T& time, } std::vector> derivative_control_points( num_control_points() - derivative_order, - MatrixX::Zero(rows(), cols())); + MatrixX::Zero(this->rows(), this->cols())); for (int i : lower_order_basis.ComputeActiveBasisFunctionIndices(clamped_time)) { derivative_control_points.at(i) = coefficients.at(i); @@ -100,7 +100,8 @@ std::unique_ptr> BsplineTrajectory::DoMakeDerivative( std::vector derivative_knots; derivative_knots.push_back(basis_.knots().front()); derivative_knots.push_back(basis_.knots().back()); - std::vector> control_points(1, MatrixX::Zero(rows(), cols())); + std::vector> control_points( + 1, MatrixX::Zero(this->rows(), this->cols())); return std::make_unique>( BsplineBasis(1, derivative_knots), control_points); } else if (derivative_order > 1) { @@ -133,12 +134,12 @@ std::unique_ptr> BsplineTrajectory::DoMakeDerivative( template MatrixX BsplineTrajectory::InitialValue() const { - return value(start_time()); + return value(this->start_time()); } template MatrixX BsplineTrajectory::FinalValue() const { - return value(end_time()); + return value(this->end_time()); } template @@ -157,7 +158,7 @@ void BsplineTrajectory::InsertKnots(const std::vector& additional_knots) { const std::vector& t = basis_.knots(); const T& t_bar = additional_knots.front(); const int k = basis_.order(); - DRAKE_DEMAND(start_time() <= t_bar && t_bar <= end_time()); + DRAKE_DEMAND(this->start_time() <= t_bar && t_bar <= this->end_time()); /* Find the index, 𝑙, of the greatest knot that is less than or equal to t_bar and strictly less than end_time(). */ @@ -218,7 +219,7 @@ BsplineTrajectory BsplineTrajectory::CopyBlock(int start_row, template BsplineTrajectory BsplineTrajectory::CopyHead(int n) const { - DRAKE_DEMAND(cols() == 1); + DRAKE_DEMAND(this->cols() == 1); DRAKE_DEMAND(n > 0); return CopyBlock(0, 0, n, 1); } diff --git a/common/trajectories/bspline_trajectory.h b/common/trajectories/bspline_trajectory.h index ff7103ef2244..137205b275ab 100644 --- a/common/trajectories/bspline_trajectory.h +++ b/common/trajectories/bspline_trajectory.h @@ -45,10 +45,7 @@ class BsplineTrajectory final : public trajectories::Trajectory { : BsplineTrajectory(math::BsplineBasis(basis), control_points) {} #endif - ~BsplineTrajectory() override; - - // Required methods for trajectories::Trajectory interface. - std::unique_ptr> Clone() const override; + ~BsplineTrajectory() final; /** Evaluates the BsplineTrajectory at the given time t. @param t The time at which to evaluate the %BsplineTrajectory. @@ -58,17 +55,11 @@ class BsplineTrajectory final : public trajectories::Trajectory { trajectory will silently be evaluated at the closest valid value of time to t. For example, `value(-1)` will return `value(0)` for a trajectory defined over [0, 1]. */ - MatrixX value(const T& time) const override; - - Eigen::Index rows() const override { return control_points()[0].rows(); } - - Eigen::Index cols() const override { return control_points()[0].cols(); } - - T start_time() const override { return basis_.initial_parameter_value(); } - - T end_time() const override { return basis_.final_parameter_value(); } + MatrixX value(const T& t) const final { + // We shadowed the base class to add documentation, not to change logic. + return Trajectory::value(t); + } - // Other methods /** Returns the number of control points in this curve. */ int num_control_points() const { return basis_.num_basis_functions(); } @@ -135,12 +126,17 @@ class BsplineTrajectory final : public trajectories::Trajectory { } private: - bool do_has_derivative() const override; - - MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; - + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final; + bool do_has_derivative() const final; + MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; std::unique_ptr> DoMakeDerivative( - int derivative_order) const override; + int derivative_order) const final; + Eigen::Index do_rows() const final { return control_points()[0].rows(); } + Eigen::Index do_cols() const final { return control_points()[0].cols(); } + T do_start_time() const final { return basis_.initial_parameter_value(); } + T do_end_time() const final { return basis_.final_parameter_value(); } void CheckInvariants() const; diff --git a/common/trajectories/composite_trajectory.cc b/common/trajectories/composite_trajectory.cc index 0a134cd2c831..c583410a4162 100644 --- a/common/trajectories/composite_trajectory.cc +++ b/common/trajectories/composite_trajectory.cc @@ -51,32 +51,12 @@ template CompositeTrajectory::~CompositeTrajectory() = default; template -std::unique_ptr> CompositeTrajectory::Clone() const { +std::unique_ptr> CompositeTrajectory::DoClone() const { return std::make_unique>(segments_); } template -Eigen::Index CompositeTrajectory::rows() const { - if (segments_.size() > 0) { - return segments_[0]->rows(); - } else { - throw std::runtime_error( - "CompositeTrajectory has no segments. Number of rows is undefined."); - } -} - -template -Eigen::Index CompositeTrajectory::cols() const { - if (segments_.size() > 0) { - return segments_[0]->cols(); - } else { - throw std::runtime_error( - "CompositeTrajectory has no segments. Number of cols is undefined."); - } -} - -template -MatrixX CompositeTrajectory::value(const T& time) const { +MatrixX CompositeTrajectory::do_value(const T& time) const { const int segment_index = this->get_segment_index(time); DRAKE_DEMAND(static_cast(segments_.size()) > segment_index); return this->segments_[segment_index]->value(time); @@ -113,6 +93,26 @@ std::unique_ptr> CompositeTrajectory::DoMakeDerivative( return std::make_unique>(std::move(derivative_curves)); } +template +Eigen::Index CompositeTrajectory::do_rows() const { + if (segments_.size() > 0) { + return segments_[0]->rows(); + } else { + throw std::runtime_error( + "CompositeTrajectory has no segments. Number of rows is undefined."); + } +} + +template +Eigen::Index CompositeTrajectory::do_cols() const { + if (segments_.size() > 0) { + return segments_[0]->cols(); + } else { + throw std::runtime_error( + "CompositeTrajectory has no segments. Number of cols is undefined."); + } +} + template CompositeTrajectory CompositeTrajectory::AlignAndConcatenate( const std::vector>>& segments) { diff --git a/common/trajectories/composite_trajectory.h b/common/trajectories/composite_trajectory.h index a8599dca0b85..567ffee2719a 100644 --- a/common/trajectories/composite_trajectory.h +++ b/common/trajectories/composite_trajectory.h @@ -33,18 +33,15 @@ class CompositeTrajectory final : public trajectories::PiecewiseTrajectory { ~CompositeTrajectory() final; - std::unique_ptr> Clone() const final; - /** Evaluates the curve at the given time. @warning If t does not lie in the range [start_time(), end_time()], the trajectory will silently be evaluated at the closest valid value of time to `time`. For example, `value(-1)` will return `value(0)` for a trajectory defined over [0, 1]. */ - MatrixX value(const T& time) const final; - - Eigen::Index rows() const final; - - Eigen::Index cols() const final; + MatrixX value(const T& t) const final { + // We shadowed the base class to add documentation, not to change logic. + return PiecewiseTrajectory::value(t); + } /** Returns a reference to the `segment_index` trajectory. */ const Trajectory& segment(int segment_index) const { @@ -62,12 +59,15 @@ class CompositeTrajectory final : public trajectories::PiecewiseTrajectory { const std::vector>>& segments); private: + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final; bool do_has_derivative() const final; - MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; - std::unique_ptr> DoMakeDerivative( int derivative_order) const final; + Eigen::Index do_rows() const final; + Eigen::Index do_cols() const final; std::vector>> segments_{}; }; diff --git a/common/trajectories/derivative_trajectory.cc b/common/trajectories/derivative_trajectory.cc index c4a206d0bc07..607fc64dfbdc 100644 --- a/common/trajectories/derivative_trajectory.cc +++ b/common/trajectories/derivative_trajectory.cc @@ -21,46 +21,46 @@ template DerivativeTrajectory::~DerivativeTrajectory() = default; template -std::unique_ptr> DerivativeTrajectory::Clone() const { +std::unique_ptr> DerivativeTrajectory::DoClone() const { return std::make_unique(*nominal_, derivative_order_); } template -MatrixX DerivativeTrajectory::value(const T& t) const { +MatrixX DerivativeTrajectory::do_value(const T& t) const { return nominal_->EvalDerivative(t, derivative_order_); } template -Eigen::Index DerivativeTrajectory::rows() const { - return rows_; +MatrixX DerivativeTrajectory::DoEvalDerivative( + const T& t, int derivative_order) const { + return nominal_->EvalDerivative(t, derivative_order_ + derivative_order); } template -Eigen::Index DerivativeTrajectory::cols() const { - return cols_; +std::unique_ptr> DerivativeTrajectory::DoMakeDerivative( + int derivative_order) const { + return std::make_unique>( + *nominal_, derivative_order_ + derivative_order); } template -T DerivativeTrajectory::start_time() const { - return nominal_->start_time(); +Eigen::Index DerivativeTrajectory::do_rows() const { + return rows_; } template -T DerivativeTrajectory::end_time() const { - return nominal_->end_time(); +Eigen::Index DerivativeTrajectory::do_cols() const { + return cols_; } template -MatrixX DerivativeTrajectory::DoEvalDerivative( - const T& t, int derivative_order) const { - return nominal_->EvalDerivative(t, derivative_order_ + derivative_order); +T DerivativeTrajectory::do_start_time() const { + return nominal_->start_time(); } template -std::unique_ptr> DerivativeTrajectory::DoMakeDerivative( - int derivative_order) const { - return std::make_unique>( - *nominal_, derivative_order_ + derivative_order); +T DerivativeTrajectory::do_end_time() const { + return nominal_->end_time(); } } // namespace trajectories diff --git a/common/trajectories/derivative_trajectory.h b/common/trajectories/derivative_trajectory.h index 88a2360e2201..fc3920cd302c 100644 --- a/common/trajectories/derivative_trajectory.h +++ b/common/trajectories/derivative_trajectory.h @@ -39,21 +39,18 @@ class DerivativeTrajectory final : public Trajectory { ~DerivativeTrajectory() final; - // Trajectory overrides. - std::unique_ptr> Clone() const final; - MatrixX value(const T& t) const final; - Eigen::Index rows() const final; - Eigen::Index cols() const final; - T start_time() const final; - T end_time() const final; - private: // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final; bool do_has_derivative() const final { return true; } - MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; std::unique_ptr> DoMakeDerivative( int derivative_order) const final; + Eigen::Index do_rows() const final; + Eigen::Index do_cols() const final; + T do_start_time() const final; + T do_end_time() const final; copyable_unique_ptr> nominal_; reset_after_move derivative_order_; diff --git a/common/trajectories/discrete_time_trajectory.cc b/common/trajectories/discrete_time_trajectory.cc index 05bfadd7825c..61f5dda76064 100644 --- a/common/trajectories/discrete_time_trajectory.cc +++ b/common/trajectories/discrete_time_trajectory.cc @@ -67,13 +67,13 @@ const std::vector& DiscreteTimeTrajectory::get_times() const { } template -std::unique_ptr> DiscreteTimeTrajectory::Clone() const { +std::unique_ptr> DiscreteTimeTrajectory::DoClone() const { return std::make_unique>( times_, values_, time_comparison_tolerance_); } template -MatrixX DiscreteTimeTrajectory::value(const T& t) const { +MatrixX DiscreteTimeTrajectory::do_value(const T& t) const { using std::abs; const double time = ExtractDoubleOrThrow(t); static constexpr const char* kNoMatchingTimeStr = @@ -93,25 +93,25 @@ MatrixX DiscreteTimeTrajectory::value(const T& t) const { } template -Eigen::Index DiscreteTimeTrajectory::rows() const { +Eigen::Index DiscreteTimeTrajectory::do_rows() const { DRAKE_DEMAND(times_.size() > 0); return values_[0].rows(); } template -Eigen::Index DiscreteTimeTrajectory::cols() const { +Eigen::Index DiscreteTimeTrajectory::do_cols() const { DRAKE_DEMAND(times_.size() > 0); return values_[0].cols(); } template -T DiscreteTimeTrajectory::start_time() const { +T DiscreteTimeTrajectory::do_start_time() const { DRAKE_DEMAND(times_.size() > 0); return times_[0]; } template -T DiscreteTimeTrajectory::end_time() const { +T DiscreteTimeTrajectory::do_end_time() const { DRAKE_DEMAND(times_.size() > 0); return times_[times_.size() - 1]; } diff --git a/common/trajectories/discrete_time_trajectory.h b/common/trajectories/discrete_time_trajectory.h index 535048715b9b..26fc89f81aee 100644 --- a/common/trajectories/discrete_time_trajectory.h +++ b/common/trajectories/discrete_time_trajectory.h @@ -86,7 +86,7 @@ class DiscreteTimeTrajectory final : public Trajectory { double time_comparison_tolerance = std::numeric_limits::epsilon()); - ~DiscreteTimeTrajectory() override; + ~DiscreteTimeTrajectory() final; /** Converts the discrete-time trajectory using PiecewisePolynomial::ZeroOrderHold(). */ @@ -104,31 +104,51 @@ class DiscreteTimeTrajectory final : public Trajectory { /** Returns the times where the trajectory value is defined. */ const std::vector& get_times() const; - /** Returns a deep copy of the trajectory. */ - std::unique_ptr> Clone() const override; - /** Returns the value of the trajectory at @p t. @throws std::exception if t is not within tolerance of one of the sample times. */ - MatrixX value(const T& t) const override; + MatrixX value(const T& t) const final { + // We shadowed the base class to add documentation, not to change logic. + return Trajectory::value(t); + } /** Returns the number of rows in the MatrixX returned by value(). @pre num_times() > 0. */ - Eigen::Index rows() const override; + Eigen::Index rows() const final { + // We shadowed the base class to add documentation, not to change logic. + return Trajectory::rows(); + } /** Returns the number of cols in the MatrixX returned by value(). @pre num_times() > 0. */ - Eigen::Index cols() const override; + Eigen::Index cols() const final { + // We shadowed the base class to add documentation, not to change logic. + return Trajectory::cols(); + } /** Returns the minimum value of get_times(). @pre num_times() > 0. */ - T start_time() const override; + T start_time() const final { + // We shadowed the base class to add documentation, not to change logic. + return Trajectory::start_time(); + } /** Returns the maximum value of get_times(). @pre num_times() > 0. */ - T end_time() const override; + T end_time() const final { + // We shadowed the base class to add documentation, not to change logic. + return Trajectory::end_time(); + } private: + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final; + Eigen::Index do_rows() const final; + Eigen::Index do_cols() const final; + T do_start_time() const final; + T do_end_time() const final; + std::vector times_; std::vector> values_; double time_comparison_tolerance_{}; diff --git a/common/trajectories/exponential_plus_piecewise_polynomial.cc b/common/trajectories/exponential_plus_piecewise_polynomial.cc index 8abde065ea00..e202d894d693 100644 --- a/common/trajectories/exponential_plus_piecewise_polynomial.cc +++ b/common/trajectories/exponential_plus_piecewise_polynomial.cc @@ -27,22 +27,6 @@ template ExponentialPlusPiecewisePolynomial::~ExponentialPlusPiecewisePolynomial() = default; -template -std::unique_ptr> ExponentialPlusPiecewisePolynomial::Clone() - const { - return std::make_unique>(*this); -} - -template -MatrixX ExponentialPlusPiecewisePolynomial::value(const T& t) const { - int segment_index = this->get_segment_index(t); - MatrixX ret = piecewise_polynomial_part_.value(t); - double tj = this->start_time(segment_index); - auto exponential = (A_ * (t - tj)).eval().exp().eval(); - ret.noalias() += K_ * exponential * alpha_.col(segment_index); - return ret; -} - template ExponentialPlusPiecewisePolynomial ExponentialPlusPiecewisePolynomial::derivative(int derivative_order) const { @@ -58,16 +42,6 @@ ExponentialPlusPiecewisePolynomial::derivative(int derivative_order) const { piecewise_polynomial_part_.derivative(derivative_order)); } -template -Eigen::Index ExponentialPlusPiecewisePolynomial::rows() const { - return piecewise_polynomial_part_.rows(); -} - -template -Eigen::Index ExponentialPlusPiecewisePolynomial::cols() const { - return piecewise_polynomial_part_.cols(); -} - template void ExponentialPlusPiecewisePolynomial::shiftRight(double offset) { std::vector& breaks = this->get_mutable_breaks(); @@ -77,6 +51,32 @@ void ExponentialPlusPiecewisePolynomial::shiftRight(double offset) { piecewise_polynomial_part_.shiftRight(offset); } +template +std::unique_ptr> ExponentialPlusPiecewisePolynomial::DoClone() + const { + return std::make_unique>(*this); +} + +template +MatrixX ExponentialPlusPiecewisePolynomial::do_value(const T& t) const { + int segment_index = this->get_segment_index(t); + MatrixX ret = piecewise_polynomial_part_.value(t); + double tj = this->start_time(segment_index); + auto exponential = (A_ * (t - tj)).eval().exp().eval(); + ret.noalias() += K_ * exponential * alpha_.col(segment_index); + return ret; +} + +template +Eigen::Index ExponentialPlusPiecewisePolynomial::do_rows() const { + return piecewise_polynomial_part_.rows(); +} + +template +Eigen::Index ExponentialPlusPiecewisePolynomial::do_cols() const { + return piecewise_polynomial_part_.cols(); +} + template class ExponentialPlusPiecewisePolynomial; } // namespace trajectories diff --git a/common/trajectories/exponential_plus_piecewise_polynomial.h b/common/trajectories/exponential_plus_piecewise_polynomial.h index 9ad5684fa7af..0cd5d69e1ad1 100644 --- a/common/trajectories/exponential_plus_piecewise_polynomial.h +++ b/common/trajectories/exponential_plus_piecewise_polynomial.h @@ -55,13 +55,13 @@ class ExponentialPlusPiecewisePolynomial final : public PiecewiseTrajectory { A_(A), alpha_(alpha), piecewise_polynomial_part_(piecewise_polynomial_part) { - DRAKE_ASSERT(K.rows() == rows()); + DRAKE_ASSERT(K.rows() == this->rows()); DRAKE_ASSERT(K.cols() == A.rows()); DRAKE_ASSERT(A.rows() == A.cols()); DRAKE_ASSERT(alpha.rows() == A.cols()); DRAKE_ASSERT(alpha.cols() == piecewise_polynomial_part.get_number_of_segments()); - DRAKE_ASSERT(piecewise_polynomial_part.rows() == rows()); + DRAKE_ASSERT(piecewise_polynomial_part.rows() == this->rows()); DRAKE_ASSERT(piecewise_polynomial_part.cols() == 1); } @@ -69,25 +69,18 @@ class ExponentialPlusPiecewisePolynomial final : public PiecewiseTrajectory { ExponentialPlusPiecewisePolynomial( const PiecewisePolynomial& piecewise_polynomial_part); - ~ExponentialPlusPiecewisePolynomial() override; - - std::unique_ptr> Clone() const override; - - MatrixX value(const T& t) const override; + ~ExponentialPlusPiecewisePolynomial() final; ExponentialPlusPiecewisePolynomial derivative(int derivative_order = 1) const; - Eigen::Index rows() const override; - - Eigen::Index cols() const override; - void shiftRight(double offset); private: - std::unique_ptr> DoMakeDerivative( - int derivative_order = 1) const override { - return derivative(derivative_order).Clone(); - }; + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final; + Eigen::Index do_rows() const final; + Eigen::Index do_cols() const final; MatrixX K_; MatrixX A_; diff --git a/common/trajectories/function_handle_trajectory.cc b/common/trajectories/function_handle_trajectory.cc index d8e7e073e493..29af79f62c91 100644 --- a/common/trajectories/function_handle_trajectory.cc +++ b/common/trajectories/function_handle_trajectory.cc @@ -29,14 +29,14 @@ FunctionHandleTrajectory::FunctionHandleTrajectory( // Call value() once to confirm that function returns the correct size. We // must do the check in the value() method, because the func_ could produce // differently sized outputs at different times. - value(start_time); + this->value(start_time); } template FunctionHandleTrajectory::~FunctionHandleTrajectory() = default; template -std::unique_ptr> FunctionHandleTrajectory::Clone() const { +std::unique_ptr> FunctionHandleTrajectory::DoClone() const { using Self = FunctionHandleTrajectory; auto clone = std::make_unique(func_, rows_, cols_, start_time_, end_time_); @@ -45,7 +45,7 @@ std::unique_ptr> FunctionHandleTrajectory::Clone() const { } template -MatrixX FunctionHandleTrajectory::value(const T& t) const { +MatrixX FunctionHandleTrajectory::do_value(const T& t) const { if (rows_ == 0 || cols_ == 0) { // Handle the empty (default) case; which can happen after a move operation. return MatrixX::Zero(0, 0); @@ -65,7 +65,7 @@ template MatrixX FunctionHandleTrajectory::DoEvalDerivative( const T& t, int derivative_order) const { if (derivative_order == 0) { - return value(t); + return this->value(t); } DRAKE_THROW_UNLESS(derivative_func_ != nullptr); MatrixX derivative = derivative_func_(t, derivative_order); @@ -83,7 +83,7 @@ template std::unique_ptr> FunctionHandleTrajectory::DoMakeDerivative( int derivative_order) const { if (derivative_order == 0) { - return Clone(); + return this->Clone(); } DRAKE_THROW_UNLESS(derivative_func_ != nullptr); return std::make_unique>(*this, derivative_order); diff --git a/common/trajectories/function_handle_trajectory.h b/common/trajectories/function_handle_trajectory.h index dbfbad61911c..c3bde990fda2 100644 --- a/common/trajectories/function_handle_trajectory.h +++ b/common/trajectories/function_handle_trajectory.h @@ -58,26 +58,22 @@ class FunctionHandleTrajectory final : public Trajectory { derivative_func_ = func; } - // Trajectory overrides. - std::unique_ptr> Clone() const final; - MatrixX value(const T& t) const final; - Eigen::Index rows() const final { return rows_; }; - Eigen::Index cols() const final { return cols_; }; - T start_time() const final { return start_time_; }; - T end_time() const final { return end_time_; }; - private: // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final; bool do_has_derivative() const final { return derivative_func_ != nullptr; } - // This method throws a std::exception if derivative_order != 0 and // derivative_func_ == nullptr. - MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; - + MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; // This method throws a std::exception if derivative_order != 0 and // derivative_func_ == nullptr. std::unique_ptr> DoMakeDerivative( - int derivative_order) const override; + int derivative_order) const final; + Eigen::Index do_rows() const final { return rows_; }; + Eigen::Index do_cols() const final { return cols_; }; + T do_start_time() const final { return start_time_; }; + T do_end_time() const final { return end_time_; }; std::function(const T&)> func_{}; std::function(const T&, int)> derivative_func_{}; diff --git a/common/trajectories/path_parameterized_trajectory.cc b/common/trajectories/path_parameterized_trajectory.cc index 36e9cb9f7388..144fbf810c68 100644 --- a/common/trajectories/path_parameterized_trajectory.cc +++ b/common/trajectories/path_parameterized_trajectory.cc @@ -22,12 +22,12 @@ template PathParameterizedTrajectory::~PathParameterizedTrajectory() = default; template -std::unique_ptr> PathParameterizedTrajectory::Clone() const { +std::unique_ptr> PathParameterizedTrajectory::DoClone() const { return std::make_unique>(*this); } template -MatrixX PathParameterizedTrajectory::value(const T& t) const { +MatrixX PathParameterizedTrajectory::do_value(const T& t) const { using std::clamp; const T time = clamp(t, time_scaling_->start_time(), time_scaling_->end_time()); @@ -67,7 +67,7 @@ MatrixX PathParameterizedTrajectory::DoEvalDerivative( } // Derivative is calculated using Faà di Bruno's formula with Bell // polynomials: https://en.wikipedia.org/wiki/Fa%C3%A0_di_Bruno%27s_formula - MatrixX derivative = MatrixX::Zero(rows(), cols()); + MatrixX derivative = MatrixX::Zero(this->rows(), this->cols()); for (int order = 1; order <= derivative_order; ++order) { MatrixX path_partial = path_->EvalDerivative(time_scaling_->value(time)(0, 0), order); diff --git a/common/trajectories/path_parameterized_trajectory.h b/common/trajectories/path_parameterized_trajectory.h index a6d9d26ac53e..e037d7f6650e 100644 --- a/common/trajectories/path_parameterized_trajectory.h +++ b/common/trajectories/path_parameterized_trajectory.h @@ -32,9 +32,6 @@ class PathParameterizedTrajectory final : public Trajectory { ~PathParameterizedTrajectory() final; - // Required methods for trajectories::Trajectory interface. - std::unique_ptr> Clone() const override; - /** Evaluates the PathParameterizedTrajectory at the given time t. @param t The time at which to evaluate the %PathParameterizedTrajectory. @return The matrix of evaluated values. @@ -43,15 +40,10 @@ class PathParameterizedTrajectory final : public Trajectory { trajectory will silently be evaluated at the closest valid value of time to t. For example, `value(-1)` will return `value(0)` for a trajectory defined over [0, 1]. */ - MatrixX value(const T& t) const override; - - Eigen::Index rows() const override { return path_->rows(); } - - Eigen::Index cols() const override { return path_->cols(); } - - T start_time() const override { return time_scaling_->start_time(); } - - T end_time() const override { return time_scaling_->end_time(); } + MatrixX value(const T& t) const final { + // We shadowed the base class to add documentation, not to change logic. + return Trajectory::value(t); + } /** Returns the path of this trajectory. */ const trajectories::Trajectory& path() const { return *path_; } @@ -62,27 +54,25 @@ class PathParameterizedTrajectory final : public Trajectory { } private: - // Evaluates the %PathParameterizedTrajectory derivative at the given time @p - // t. Returns the nth derivative, where `n` is the value of @p - // derivative_order. - // - // @warning This method comes with the same caveats as value(). See value() - // @pre derivative_order must be non-negative. - MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; - - // Uses DerivativeTrajectory to provide a derivative object. + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final; + bool do_has_derivative() const final { + return path_->has_derivative() && time_scaling_->has_derivative(); + } + MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; std::unique_ptr> DoMakeDerivative( int derivative_order) const final; + Eigen::Index do_rows() const final { return path_->rows(); } + Eigen::Index do_cols() const final { return path_->cols(); } + T do_start_time() const final { return time_scaling_->start_time(); } + T do_end_time() const final { return time_scaling_->end_time(); } // Evaluates the Bell Polynomial B_n,k(x) for use in calculating the // derivative. // @pre n and k must be non-negative and the length of x must be at least n. T BellPolynomial(int n, int k, const VectorX& x) const; - bool do_has_derivative() const override { - return path_->has_derivative() && time_scaling_->has_derivative(); - } - copyable_unique_ptr> path_; copyable_unique_ptr> time_scaling_; }; diff --git a/common/trajectories/piecewise_constant_curvature_trajectory.cc b/common/trajectories/piecewise_constant_curvature_trajectory.cc index ed09d822d48d..cb06664b384b 100644 --- a/common/trajectories/piecewise_constant_curvature_trajectory.cc +++ b/common/trajectories/piecewise_constant_curvature_trajectory.cc @@ -39,17 +39,6 @@ PiecewiseConstantCurvatureTrajectory::PiecewiseConstantCurvatureTrajectory( breaks, turning_rates); } -template -std::unique_ptr> PiecewiseConstantCurvatureTrajectory::Clone() - const { - auto initial_pose = get_initial_pose(); - auto initial_frame = initial_pose.rotation(); - return std::make_unique>( - this->breaks(), segment_turning_rates_, - initial_frame.col(kCurveTangentIndex), - initial_frame.col(kPlaneNormalIndex), initial_pose.translation()); -} - template math::RigidTransform PiecewiseConstantCurvatureTrajectory::CalcPose( const T& s) const { @@ -115,6 +104,17 @@ boolean PiecewiseConstantCurvatureTrajectory::IsNearlyPeriodic( return CalcPose(0.).IsNearlyEqualTo(CalcPose(length()), tolerance); } +template +std::unique_ptr> +PiecewiseConstantCurvatureTrajectory::DoClone() const { + auto initial_pose = get_initial_pose(); + auto initial_frame = initial_pose.rotation(); + return std::make_unique>( + this->breaks(), segment_turning_rates_, + initial_frame.col(kCurveTangentIndex), + initial_frame.col(kPlaneNormalIndex), initial_pose.translation()); +} + template math::RigidTransform PiecewiseConstantCurvatureTrajectory::CalcRelativePoseInSegment( diff --git a/common/trajectories/piecewise_constant_curvature_trajectory.h b/common/trajectories/piecewise_constant_curvature_trajectory.h index 8ac88bf949ef..e25c28588518 100644 --- a/common/trajectories/piecewise_constant_curvature_trajectory.h +++ b/common/trajectories/piecewise_constant_curvature_trajectory.h @@ -125,12 +125,6 @@ class PiecewiseConstantCurvatureTrajectory final .template cast(), other.get_initial_pose().translation().template cast()) {} - /** @returns the number of rows in the output of value(). */ - Eigen::Index rows() const override { return 3; } - - /** @returns the number of columns in the output of value(). */ - Eigen::Index cols() const override { return 1; } - /** @returns the total arclength of the curve in meters. */ T length() const { return this->end_time(); } @@ -148,13 +142,11 @@ class PiecewiseConstantCurvatureTrajectory final @param s The query arclength in meters. @returns position vector r(s). */ - MatrixX value(const T& s) const override { - return CalcPose(s).translation(); + MatrixX value(const T& s) const final { + // We shadowed the base class to add documentation, not to change logic. + return PiecewiseTrajectory::value(s); } - /** @returns a deep copy of `this` trajectory. */ - std::unique_ptr> Clone() const override; - /** Computes the spatial velocity V_AF_A(s) of frame F measured and expressed in the reference frame A. See the class's documentation for frame definitions. @@ -214,6 +206,14 @@ class PiecewiseConstantCurvatureTrajectory final template friend class PiecewiseConstantCurvatureTrajectory; + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& s) const final { + return CalcPose(s).translation(); + } + Eigen::Index do_rows() const final { return 3; } + Eigen::Index do_cols() const final { return 1; } + /* Helper function to scalar convert a std::vector into a std::vector. @param segment_data std::vector storing types U. @returns the input vector scalar converted T. */ diff --git a/common/trajectories/piecewise_polynomial.cc b/common/trajectories/piecewise_polynomial.cc index d44f32aebcaa..2a0e65951bda 100644 --- a/common/trajectories/piecewise_polynomial.cc +++ b/common/trajectories/piecewise_polynomial.cc @@ -61,7 +61,7 @@ template PiecewisePolynomial::~PiecewisePolynomial() = default; template -std::unique_ptr> PiecewisePolynomial::Clone() const { +std::unique_ptr> PiecewisePolynomial::DoClone() const { return std::make_unique>(*this); } @@ -648,7 +648,7 @@ T PiecewisePolynomial::EvaluateSegmentAbsoluteTime( } template -Eigen::Index PiecewisePolynomial::rows() const { +Eigen::Index PiecewisePolynomial::do_rows() const { if (polynomials_.size() > 0) { return polynomials_[0].rows(); } else { @@ -658,7 +658,7 @@ Eigen::Index PiecewisePolynomial::rows() const { } template -Eigen::Index PiecewisePolynomial::cols() const { +Eigen::Index PiecewisePolynomial::do_cols() const { if (polynomials_.size() > 0) { return polynomials_[0].cols(); } else { diff --git a/common/trajectories/piecewise_polynomial.h b/common/trajectories/piecewise_polynomial.h index ef12a9b978e5..a3970bbe9b46 100644 --- a/common/trajectories/piecewise_polynomial.h +++ b/common/trajectories/piecewise_polynomial.h @@ -181,9 +181,7 @@ class PiecewisePolynomial final : public PiecewiseTrajectory { const std::vector& breaks); // @} - ~PiecewisePolynomial() override; - - std::unique_ptr> Clone() const override; + ~PiecewisePolynomial() final; /** * @anchor coefficient_construction_methods @@ -512,9 +510,9 @@ class PiecewisePolynomial final : public PiecewiseTrajectory { * @warning See warning in the @ref polynomial_warning "constructor overview" * above. */ - MatrixX value(const T& t) const override { - const int derivative_order = 0; - return DoEvalDerivative(t, derivative_order); + MatrixX value(const T& t) const final { + // We shadowed the base class to add documentation, not to change logic. + return PiecewiseTrajectory::value(t); } /** @@ -545,13 +543,19 @@ class PiecewisePolynomial final : public PiecewiseTrajectory { * Returns the row count of the output matrices. * @throws std::exception if empty(). */ - Eigen::Index rows() const override; + Eigen::Index rows() const final { + // We shadowed the base class to add documentation, not to change logic. + return PiecewiseTrajectory::rows(); + } /** * Returns the column count of the output matrices. * @throws std::exception if empty(). */ - Eigen::Index cols() const override; + Eigen::Index cols() const final { + // We shadowed the base class to add documentation, not to change logic. + return PiecewiseTrajectory::cols(); + } /** * Reshapes the dimensions of the Eigen::MatrixX returned by value(), @@ -804,19 +808,25 @@ class PiecewisePolynomial final : public PiecewiseTrajectory { } private: + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final { + const int derivative_order = 0; + return DoEvalDerivative(t, derivative_order); + } // Evaluates the %PiecwisePolynomial derivative at the given time @p t. // Returns the nth derivative, where `n` is the value of @p derivative_order. // // @warning This method comes with the same caveats as value(). See value() // @pre derivative_order must be non-negative. - MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; - + MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; std::unique_ptr> DoMakeDerivative( - int derivative_order) const override { + int derivative_order) const final { return derivative(derivative_order).Clone(); } - - bool do_has_derivative() const override { return true; } + bool do_has_derivative() const final { return true; } + Eigen::Index do_rows() const final; + Eigen::Index do_cols() const final; T EvaluateSegmentAbsoluteTime(int segment_index, const T& t, Eigen::Index row, Eigen::Index col, diff --git a/common/trajectories/piecewise_pose.cc b/common/trajectories/piecewise_pose.cc index 28a52bda340e..5cb270b731b7 100644 --- a/common/trajectories/piecewise_pose.cc +++ b/common/trajectories/piecewise_pose.cc @@ -58,11 +58,6 @@ PiecewisePose PiecewisePose::MakeCubicLinearWithEndLinearVelocity( PiecewiseQuaternionSlerp(times, rot_knots)); } -template -std::unique_ptr> PiecewisePose::Clone() const { - return std::make_unique(*this); -} - template math::RigidTransform PiecewisePose::GetPose(const T& time) const { return math::RigidTransform(orientation_.orientation(time), @@ -109,6 +104,11 @@ bool PiecewisePose::IsApprox(const PiecewisePose& other, return ret; } +template +std::unique_ptr> PiecewisePose::DoClone() const { + return std::make_unique(*this); +} + template bool PiecewisePose::do_has_derivative() const { return true; @@ -118,7 +118,7 @@ template MatrixX PiecewisePose::DoEvalDerivative(const T& t, int derivative_order) const { if (derivative_order == 0) { - return value(t); + return this->value(t); } Vector6 derivative; derivative.template head<3>() = diff --git a/common/trajectories/piecewise_pose.h b/common/trajectories/piecewise_pose.h index 23f9b2c9892c..cc6ad4b663f5 100644 --- a/common/trajectories/piecewise_pose.h +++ b/common/trajectories/piecewise_pose.h @@ -37,7 +37,7 @@ class PiecewisePose final : public PiecewiseTrajectory { PiecewisePose(const PiecewisePolynomial& position_trajectory, const PiecewiseQuaternionSlerp& orientation_trajectory); - ~PiecewisePose() override; + ~PiecewisePose() final; /** * Constructs a PiecewisePose from given @p times and @p poses. The positions @@ -67,21 +67,11 @@ class PiecewisePose final : public PiecewiseTrajectory { const Vector3& start_vel = Vector3::Zero(), const Vector3& end_vel = Vector3::Zero()); - std::unique_ptr> Clone() const override; - - Eigen::Index rows() const override { return 4; } - - Eigen::Index cols() const override { return 4; } - /** * Returns the interpolated pose at @p time. */ math::RigidTransform GetPose(const T& time) const; - MatrixX value(const T& t) const override { - return GetPose(t).GetAsMatrix4(); - } - /** * Returns the interpolated velocity at @p time or zero if @p time is before * this trajectory's start time or after its end time. @@ -115,12 +105,17 @@ class PiecewisePose final : public PiecewiseTrajectory { } private: - bool do_has_derivative() const override; - - MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; - + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final { + return GetPose(t).GetAsMatrix4(); + } + bool do_has_derivative() const final; + MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; std::unique_ptr> DoMakeDerivative( - int derivative_order) const override; + int derivative_order) const final; + Eigen::Index do_rows() const final { return 4; } + Eigen::Index do_cols() const final { return 4; } PiecewisePolynomial position_; PiecewisePolynomial velocity_; diff --git a/common/trajectories/piecewise_quaternion.cc b/common/trajectories/piecewise_quaternion.cc index 592ad0ae9c5a..cae22931303a 100644 --- a/common/trajectories/piecewise_quaternion.cc +++ b/common/trajectories/piecewise_quaternion.cc @@ -124,11 +124,6 @@ PiecewiseQuaternionSlerp::PiecewiseQuaternionSlerp( template PiecewiseQuaternionSlerp::~PiecewiseQuaternionSlerp() = default; -template -std::unique_ptr> PiecewiseQuaternionSlerp::Clone() const { - return std::make_unique(*this); -} - template T PiecewiseQuaternionSlerp::ComputeInterpTime(int segment_index, const T& time) const { @@ -191,6 +186,11 @@ void PiecewiseQuaternionSlerp::Append(const T& time, Append(time, Quaternion(angle_axis)); } +template +std::unique_ptr> PiecewiseQuaternionSlerp::DoClone() const { + return std::make_unique(*this); +} + template bool PiecewiseQuaternionSlerp::do_has_derivative() const { return true; @@ -200,7 +200,7 @@ template MatrixX PiecewiseQuaternionSlerp::DoEvalDerivative( const T& t, int derivative_order) const { if (derivative_order == 0) { - return value(t); + return this->value(t); } else if (derivative_order == 1) { return angular_velocity(t); } diff --git a/common/trajectories/piecewise_quaternion.h b/common/trajectories/piecewise_quaternion.h index 8db12cd177af..5871e320380b 100644 --- a/common/trajectories/piecewise_quaternion.h +++ b/common/trajectories/piecewise_quaternion.h @@ -74,13 +74,7 @@ class PiecewiseQuaternionSlerp final : public PiecewiseTrajectory { PiecewiseQuaternionSlerp(const std::vector& breaks, const std::vector>& angle_axes); - ~PiecewiseQuaternionSlerp() override; - - std::unique_ptr> Clone() const override; - - Eigen::Index rows() const override { return 4; } - - Eigen::Index cols() const override { return 1; } + ~PiecewiseQuaternionSlerp() final; /** * Interpolates orientation. @@ -89,11 +83,6 @@ class PiecewiseQuaternionSlerp final : public PiecewiseTrajectory { */ Quaternion orientation(const T& time) const; - MatrixX value(const T& time) const override { - const Quaternion quat = orientation(time); - return Vector4(quat.w(), quat.x(), quat.y(), quat.z()); - } - /** * Interpolates angular velocity. * @param time Time for interpolation. @@ -154,12 +143,18 @@ class PiecewiseQuaternionSlerp final : public PiecewiseTrajectory { // Computes the interpolation time within each segment. Result is in [0, 1]. T ComputeInterpTime(int segment_index, const T& time) const; - bool do_has_derivative() const override; - - MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; - + // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& time) const final { + const Quaternion quat = orientation(time); + return Vector4(quat.w(), quat.x(), quat.y(), quat.z()); + } + bool do_has_derivative() const final; + MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; std::unique_ptr> DoMakeDerivative( - int derivative_order) const override; + int derivative_order) const final; + Eigen::Index do_rows() const final { return 4; } + Eigen::Index do_cols() const final { return 1; } std::vector> quaternions_; std::vector> angular_velocities_; diff --git a/common/trajectories/piecewise_trajectory.cc b/common/trajectories/piecewise_trajectory.cc index 950d2e9fdafb..6987a5da4ab4 100644 --- a/common/trajectories/piecewise_trajectory.cc +++ b/common/trajectories/piecewise_trajectory.cc @@ -50,16 +50,6 @@ T PiecewiseTrajectory::duration(int segment_number) const { return end_time(segment_number) - start_time(segment_number); } -template -T PiecewiseTrajectory::start_time() const { - return start_time(0); -} - -template -T PiecewiseTrajectory::end_time() const { - return end_time(get_number_of_segments() - 1); -} - template int PiecewiseTrajectory::GetSegmentIndexRecursive(const T& time, int start, int end) const { @@ -121,6 +111,16 @@ std::vector PiecewiseTrajectory::RandomSegmentTimes( return breaks; } +template +T PiecewiseTrajectory::do_start_time() const { + return start_time(0); +} + +template +T PiecewiseTrajectory::do_end_time() const { + return end_time(get_number_of_segments() - 1); +} + template bool PiecewiseTrajectory::SegmentTimesEqual( const PiecewiseTrajectory& other, double tol) const { diff --git a/common/trajectories/piecewise_trajectory.h b/common/trajectories/piecewise_trajectory.h index 0251121b3869..e8b909ef0903 100644 --- a/common/trajectories/piecewise_trajectory.h +++ b/common/trajectories/piecewise_trajectory.h @@ -29,15 +29,13 @@ class PiecewiseTrajectory : public Trajectory { int get_number_of_segments() const; T start_time(int segment_number) const; + using Trajectory::start_time; // Don't shadow the base class. T end_time(int segment_number) const; + using Trajectory::end_time; // Don't shadow the base class. T duration(int segment_number) const; - T start_time() const override; - - T end_time() const override; - /** * Returns true iff `t >= getStartTime() && t <= getEndTime()`. */ @@ -61,6 +59,10 @@ class PiecewiseTrajectory : public Trajectory { /// @p breaks increments must be greater or equal to kEpsilonTime. explicit PiecewiseTrajectory(const std::vector& breaks); + // Trajectory overrides. + T do_start_time() const override; + T do_end_time() const override; + bool SegmentTimesEqual(const PiecewiseTrajectory& b, double tol = kEpsilonTime) const; diff --git a/common/trajectories/stacked_trajectory.cc b/common/trajectories/stacked_trajectory.cc index 07dbd569c121..15b731058cc0 100644 --- a/common/trajectories/stacked_trajectory.cc +++ b/common/trajectories/stacked_trajectory.cc @@ -29,15 +29,15 @@ void StackedTrajectory::Append(std::unique_ptr> traj) { // Check for valid times. if (!children_.empty()) { - DRAKE_THROW_UNLESS(traj->start_time() == start_time()); - DRAKE_THROW_UNLESS(traj->end_time() == end_time()); + DRAKE_THROW_UNLESS(traj->start_time() == this->start_time()); + DRAKE_THROW_UNLESS(traj->end_time() == this->end_time()); } // Check for valid sizes. if (rowwise_) { - DRAKE_THROW_UNLESS(children_.empty() || traj->cols() == cols()); + DRAKE_THROW_UNLESS(children_.empty() || traj->cols() == this->cols()); } else { - DRAKE_THROW_UNLESS(children_.empty() || traj->rows() == rows()); + DRAKE_THROW_UNLESS(children_.empty() || traj->rows() == this->rows()); } // Take ownership and update our internal sizes. @@ -79,13 +79,13 @@ void StackedTrajectory::CheckInvariants() const { // Sanity-check that the time span is the same for all children. for (const auto& child : children_) { - DRAKE_DEMAND(child->start_time() == start_time()); - DRAKE_DEMAND(child->end_time() == end_time()); + DRAKE_DEMAND(child->start_time() == this->start_time()); + DRAKE_DEMAND(child->end_time() == this->end_time()); } } template -std::unique_ptr> StackedTrajectory::Clone() const { +std::unique_ptr> StackedTrajectory::DoClone() const { using Self = StackedTrajectory; auto result = std::unique_ptr(new Self(*this)); DRAKE_ASSERT_VOID(CheckInvariants()); @@ -94,8 +94,8 @@ std::unique_ptr> StackedTrajectory::Clone() const { } template -MatrixX StackedTrajectory::value(const T& t) const { - MatrixX result(rows(), cols()); +MatrixX StackedTrajectory::do_value(const T& t) const { + MatrixX result(this->rows(), this->cols()); Index row = 0; Index col = 0; for (const auto& child : children_) { @@ -112,26 +112,6 @@ MatrixX StackedTrajectory::value(const T& t) const { return result; } -template -Index StackedTrajectory::rows() const { - return rows_; -} - -template -Index StackedTrajectory::cols() const { - return cols_; -} - -template -T StackedTrajectory::start_time() const { - return children_.empty() ? 0 : children_.front()->start_time(); -} - -template -T StackedTrajectory::end_time() const { - return children_.empty() ? 0 : children_.front()->end_time(); -} - template bool StackedTrajectory::do_has_derivative() const { return std::all_of(children_.begin(), children_.end(), [](const auto& child) { @@ -142,7 +122,7 @@ bool StackedTrajectory::do_has_derivative() const { template MatrixX StackedTrajectory::DoEvalDerivative(const T& t, int derivative_order) const { - MatrixX result(rows(), cols()); + MatrixX result(this->rows(), this->cols()); Index row = 0; Index col = 0; for (const auto& child : children_) { @@ -169,6 +149,26 @@ std::unique_ptr> StackedTrajectory::DoMakeDerivative( return result; } +template +Index StackedTrajectory::do_rows() const { + return rows_; +} + +template +Index StackedTrajectory::do_cols() const { + return cols_; +} + +template +T StackedTrajectory::do_start_time() const { + return children_.empty() ? 0 : children_.front()->start_time(); +} + +template +T StackedTrajectory::do_end_time() const { + return children_.empty() ? 0 : children_.front()->end_time(); +} + } // namespace trajectories } // namespace drake diff --git a/common/trajectories/stacked_trajectory.h b/common/trajectories/stacked_trajectory.h index ff1690c0484c..c4c33c5dfa5c 100644 --- a/common/trajectories/stacked_trajectory.h +++ b/common/trajectories/stacked_trajectory.h @@ -47,14 +47,6 @@ class StackedTrajectory final : public Trajectory { @throws std::exception if the matrix dimension is incompatible. */ void Append(const Trajectory& traj); - // Trajectory overrides. - std::unique_ptr> Clone() const final; - MatrixX value(const T& t) const final; - Eigen::Index rows() const final; - Eigen::Index cols() const final; - T start_time() const final; - T end_time() const final; - private: void CheckInvariants() const; @@ -62,10 +54,16 @@ class StackedTrajectory final : public Trajectory { void Append(std::unique_ptr> traj); // Trajectory overrides. + std::unique_ptr> DoClone() const final; + MatrixX do_value(const T& t) const final; bool do_has_derivative() const final; MatrixX DoEvalDerivative(const T& t, int derivative_order) const final; std::unique_ptr> DoMakeDerivative( int derivative_order) const final; + Eigen::Index do_rows() const final; + Eigen::Index do_cols() const final; + T do_start_time() const final; + T do_end_time() const final; bool rowwise_{}; std::vector>> children_; diff --git a/common/trajectories/trajectory.cc b/common/trajectories/trajectory.cc index d89afee15c82..d60f1b5ed970 100644 --- a/common/trajectories/trajectory.cc +++ b/common/trajectories/trajectory.cc @@ -2,12 +2,20 @@ #include "drake/common/unused.h" +// Remove 2025-08-01 with deprecation. +#include "drake/common/nice_type_name.h" + namespace drake { namespace trajectories { template Trajectory::~Trajectory() = default; +template +std::unique_ptr> Trajectory::Clone() const { + return DoClone(); +} + template MatrixX Trajectory::vector_values(const std::vector& t) const { return vector_values(Eigen::Map>(t.data(), t.size())); @@ -34,6 +42,23 @@ MatrixX Trajectory::vector_values( return values; } +// Switch to be pure virtual on 2025-08-01. +template +std::unique_ptr> Trajectory::DoClone() const { + throw std::logic_error( + fmt::format("{} is implemented incorrectly: it failed to override {}", + drake::NiceTypeName::Get(*this), __func__)); +} + +// Switch to be pure virtual on 2025-08-01. +template +MatrixX Trajectory::do_value(const T& t) const { + unused(t); + throw std::logic_error( + fmt::format("{} is implemented incorrectly: it failed to override {}", + drake::NiceTypeName::Get(*this), __func__)); +} + template bool Trajectory::has_derivative() const { return do_has_derivative(); @@ -89,6 +114,38 @@ std::unique_ptr> Trajectory::DoMakeDerivative( } } +// Switch to be pure virtual on 2025-08-01. +template +Eigen::Index Trajectory::do_rows() const { + throw std::logic_error( + fmt::format("{} is implemented incorrectly: it failed to override {}", + drake::NiceTypeName::Get(*this), __func__)); +} + +// Switch to be pure virtual on 2025-08-01. +template +Eigen::Index Trajectory::do_cols() const { + throw std::logic_error( + fmt::format("{} is implemented incorrectly: it failed to override {}", + drake::NiceTypeName::Get(*this), __func__)); +} + +// Switch to be pure virtual on 2025-08-01. +template +T Trajectory::do_start_time() const { + throw std::logic_error( + fmt::format("{} is implemented incorrectly: it failed to override {}", + drake::NiceTypeName::Get(*this), __func__)); +} + +// Switch to be pure virtual on 2025-08-01. +template +T Trajectory::do_end_time() const { + throw std::logic_error( + fmt::format("{} is implemented incorrectly: it failed to override {}", + drake::NiceTypeName::Get(*this), __func__)); +} + } // namespace trajectories } // namespace drake diff --git a/common/trajectories/trajectory.h b/common/trajectories/trajectory.h index c7a79d214fb5..7c8b3c80cb84 100644 --- a/common/trajectories/trajectory.h +++ b/common/trajectories/trajectory.h @@ -25,15 +25,23 @@ class Trajectory { /** * @return A deep copy of this Trajectory. + * + * @warning Support for overriding this as a virtual function is deprecated + * and will be removed on or after 2025-08-01. Subclasses should override the + * protected function DoClone(), instead. */ - virtual std::unique_ptr> Clone() const = 0; + virtual std::unique_ptr> Clone() const; /** * Evaluates the trajectory at the given time \p t. * @param t The time at which to evaluate the trajectory. * @return The matrix of evaluated values. + * + * @warning Support for overriding this as a virtual function is deprecated + * and will be removed on or after 2025-08-01. Subclasses should override the + * protected function do_value(), instead. */ - virtual MatrixX value(const T& t) const = 0; + virtual MatrixX value(const T& t) const { return do_value(t); } /** * If cols()==1, then evaluates the trajectory at each time @p t, and returns @@ -82,29 +90,65 @@ class Trajectory { /** * @return The number of rows in the matrix returned by value(). + * + * @warning Support for overriding this as a virtual function is deprecated + * and will be removed on or after 2025-08-01. Subclasses should override the + * protected function do_rows(), instead. */ - virtual Eigen::Index rows() const = 0; + virtual Eigen::Index rows() const { return do_rows(); } /** * @return The number of columns in the matrix returned by value(). + * + * @warning Support for overriding this as a virtual function is deprecated + * and will be removed on or after 2025-08-01. Subclasses should override the + * protected function do_cols(), instead. */ - virtual Eigen::Index cols() const = 0; + virtual Eigen::Index cols() const { return do_cols(); } - virtual T start_time() const = 0; + /** + * @warning Support for overriding this as a virtual function is deprecated + * and will be removed on or after 2025-08-01. Subclasses should override the + * protected function do_start_time(), instead. + */ + virtual T start_time() const { return do_start_time(); } - virtual T end_time() const = 0; + /** + * @warning Support for overriding this as a virtual function is deprecated + * and will be removed on or after 2025-08-01. Subclasses should override the + * protected function do_end_time(), instead. + */ + virtual T end_time() const { return do_end_time(); } protected: // Final subclasses are allowed to make copy/move/assign public. DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Trajectory); Trajectory() = default; + // This will become pure virtual on 2025-08-01. + virtual std::unique_ptr> DoClone() const; + + // This will become pure virtual on 2025-08-01. + virtual MatrixX do_value(const T& t) const; + virtual bool do_has_derivative() const; virtual MatrixX DoEvalDerivative(const T& t, int derivative_order) const; virtual std::unique_ptr> DoMakeDerivative( int derivative_order) const; + + // This will become pure virtual on 2025-08-01. + virtual Eigen::Index do_rows() const; + + // This will become pure virtual on 2025-08-01. + virtual Eigen::Index do_cols() const; + + // This will become pure virtual on 2025-08-01. + virtual T do_start_time() const; + + // This will become pure virtual on 2025-08-01. + virtual T do_end_time() const; }; } // namespace trajectories