Skip to content

Commit

Permalink
[trajectories] Deprecate overriding public virtual functions (#22395)
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
jwnimmer-tri authored Jan 6, 2025
1 parent 21da721 commit d97c505
Show file tree
Hide file tree
Showing 34 changed files with 628 additions and 427 deletions.
1 change: 1 addition & 0 deletions bindings/pydrake/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
80 changes: 79 additions & 1 deletion bindings/pydrake/test/trajectories_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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]])

Expand Down Expand Up @@ -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]()
Expand Down
106 changes: 68 additions & 38 deletions bindings/pydrake/trajectories_py.cc
Original file line number Diff line number Diff line change
@@ -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"
Expand Down Expand Up @@ -158,68 +160,98 @@ 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<TrajectoryPublic> {
public:
using Base = py::wrapper<TrajectoryPublic>;
using Base::Base;

// Trampoline virtual methods.
MatrixX<T> value(const T& t) const override {
PYBIND11_OVERLOAD_PURE(MatrixX<T>, Trajectory<T>, value, t);
}

T start_time() const override {
PYBIND11_OVERLOAD_PURE(T, Trajectory<T>, 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<T>, end_time);
}
// Trampoline virtual methods.

Eigen::Index rows() const override {
PYBIND11_OVERLOAD_PURE(Eigen::Index, Trajectory<T>, rows);
std::unique_ptr<Trajectory<T>> DoClone() const final {
// TODO(jwnimmer-tri) Rewrite cloning to use __deepcopy__ in lieu of
// Clone (or DoClone).
PYBIND11_OVERLOAD_PURE(
std::unique_ptr<Trajectory<T>>, Trajectory<T>, Clone);
}

Eigen::Index cols() const override {
PYBIND11_OVERLOAD_PURE(Eigen::Index, Trajectory<T>, cols);
MatrixX<T> do_value(const T& t) const final {
PYBIND11_OVERLOAD_INT(MatrixX<T>, Trajectory<T>, "do_value", t);
WarnDeprecatedOverride("value");
PYBIND11_OVERLOAD_INT(MatrixX<T>, Trajectory<T>, "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<T>, "do_has_derivative");
// If the macro did not return, use default functionality.
return Base::do_has_derivative();
}

MatrixX<T> DoEvalDerivative(
const T& t, int derivative_order) const override {
MatrixX<T> DoEvalDerivative(const T& t, int derivative_order) const final {
PYBIND11_OVERLOAD_INT(
MatrixX<T>, Trajectory<T>, "DoEvalDerivative", t, derivative_order);
// If the macro did not return, use default functionality.
return Base::DoEvalDerivative(t, derivative_order);
}

std::unique_ptr<Trajectory<T>> DoMakeDerivative(
int derivative_order) const override {
int derivative_order) const final {
PYBIND11_OVERLOAD_INT(std::unique_ptr<Trajectory<T>>, Trajectory<T>,
"DoMakeDerivative", derivative_order);
// If the macro did not return, use default functionality.
return Base::DoMakeDerivative(derivative_order);
}

std::unique_ptr<Trajectory<T>> Clone() const override {
PYBIND11_OVERLOAD_PURE(
std::unique_ptr<Trajectory<T>>, Trajectory<T>, Clone);
T do_start_time() const final {
PYBIND11_OVERLOAD_INT(T, Trajectory<T>, "do_start_time");
WarnDeprecatedOverride("start_time");
PYBIND11_OVERLOAD_INT(T, Trajectory<T>, "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<T>, "do_end_time");
WarnDeprecatedOverride("end_time");
PYBIND11_OVERLOAD_INT(T, Trajectory<T>, "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<T>, "do_rows");
WarnDeprecatedOverride("rows");
PYBIND11_OVERLOAD_INT(Eigen::Index, Trajectory<T>, "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<T>, "do_cols");
WarnDeprecatedOverride("cols");
PYBIND11_OVERLOAD_INT(Eigen::Index, Trajectory<T>, "cols");
// If the macro did not return, use default functionality.
return Base::do_cols();
}
};

Expand Down Expand Up @@ -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);
}

{
Expand Down Expand Up @@ -298,7 +335,6 @@ struct Impl {
py::arg("basis"), py::arg("control_points"), cls_doc.ctor.doc)
.def(py::init<math::BsplineBasis<T>, std::vector<MatrixX<T>>>(),
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,
Expand Down Expand Up @@ -330,8 +366,7 @@ struct Impl {
m, "DerivativeTrajectory", param, cls_doc.doc);
cls // BR
.def(py::init<const Trajectory<T>&, 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);
}

Expand All @@ -348,8 +383,7 @@ struct Impl {
py::arg("end_time") = std::numeric_limits<double>::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);
}

Expand All @@ -361,7 +395,6 @@ struct Impl {
cls // BR
.def(py::init<const Trajectory<T>&, const Trajectory<T>&>(),
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,
Expand Down Expand Up @@ -413,7 +446,6 @@ struct Impl {
.def(py::init<std::vector<Polynomial<T>> const&,
std::vector<T> 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++
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -723,7 +754,6 @@ struct Impl {
m, "StackedTrajectory", param, cls_doc.doc);
cls // BR
.def(py::init<bool>(), py::arg("rowwise") = true, cls_doc.ctor.doc)
.def("Clone", &Class::Clone, cls_doc.Clone.doc)
.def("Append",
py::overload_cast<const Trajectory<T>&>(&Class::Append),
/* N.B. We choose to omit any py::arg name here. */
Expand Down
2 changes: 2 additions & 0 deletions common/trajectories/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -214,6 +214,8 @@ drake_cc_library(
"//common:default_scalars",
"//common:essential",
"//common:unused",
# Remove with deprecation 2025-08-01.
"//common:nice_type_name",
],
)

Expand Down
Loading

0 comments on commit d97c505

Please sign in to comment.