From fe6a6f5747b084338117ddf4e9fc7e9b5d157999 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Mon, 6 Jan 2025 07:38:52 -0800 Subject: [PATCH] [py trajectories] Adjust Clone and MakeDerivative for unique_ptr Because we allow implementations of Trajectory as Python subclasses, we cannot assume that the deleter associated with a call to Clone is `delete MyClass`, so we now adjust our PyTrajectory override logic to wrap its return value in a WrappedTrajectory. The only reason the unique_ptr used to work is Drake's custom fork or pybind11 with evil hacks, which will be going away soon. We also now warn Python subclasses to implement the canonical spelling of the __deepcopy__ method, instead of overriding the public Clone method. (Overriding Clone was already documented as deprecated in a prior commit; this just adds the warning.) --- bindings/pydrake/_trajectories_extra.py | 19 +++- bindings/pydrake/test/trajectories_test.py | 39 +++++++-- bindings/pydrake/trajectories_py.cc | 86 +++++++++++++++++-- common/trajectories/BUILD.bazel | 19 ++++ .../test/wrapped_trajectory_test.cc | 57 ++++++++++++ common/trajectories/wrapped_trajectory.cc | 74 ++++++++++++++++ common/trajectories/wrapped_trajectory.h | 49 +++++++++++ 7 files changed, 327 insertions(+), 16 deletions(-) create mode 100644 common/trajectories/test/wrapped_trajectory_test.cc create mode 100644 common/trajectories/wrapped_trajectory.cc create mode 100644 common/trajectories/wrapped_trajectory.h diff --git a/bindings/pydrake/_trajectories_extra.py b/bindings/pydrake/_trajectories_extra.py index 9868ef338ed0..ea9be740d364 100644 --- a/bindings/pydrake/_trajectories_extra.py +++ b/bindings/pydrake/_trajectories_extra.py @@ -1,4 +1,7 @@ -from pydrake.common import _MangledName +from pydrake.common import ( + _MangledName, + pretty_class_name as _pretty_class_name, +) def __getattr__(name): @@ -7,3 +10,17 @@ def __getattr__(name): """ return _MangledName.module_getattr( module_name=__name__, module_globals=globals(), name=name) + + +def _wrapped_trajectory_repr(wrapped_trajectory): + cls = type(wrapped_trajectory) + return f"{_pretty_class_name(cls)}({wrapped_trajectory.unwrap()!r})" + + +def _add_repr_functions(): + for param in _WrappedTrajectory_.param_list: + cls = _WrappedTrajectory_[param] + setattr(cls, "__repr__", _wrapped_trajectory_repr) + + +_add_repr_functions() diff --git a/bindings/pydrake/test/trajectories_test.py b/bindings/pydrake/test/trajectories_test.py index 67addec942e4..dadb48bed72a 100644 --- a/bindings/pydrake/test/trajectories_test.py +++ b/bindings/pydrake/test/trajectories_test.py @@ -27,7 +27,8 @@ PiecewiseQuaternionSlerp_, StackedTrajectory_, Trajectory, - Trajectory_ + Trajectory_, + _WrappedTrajectory_, ) from pydrake.symbolic import Variable, Expression @@ -37,10 +38,12 @@ class CustomTrajectory(Trajectory): def __init__(self): Trajectory.__init__(self) - # TODO(jwnimmer-tri) Should be __deepcopy__ not Clone. - def Clone(self): + def __deepcopy__(self, memo): return CustomTrajectory() + def __repr__(self): + return "CustomTrajectory()" + def do_value(self, t): return np.array([[t + 1.0, t + 2.0]]) @@ -129,6 +132,7 @@ def test_custom_trajectory(self): self.assertEqual(trajectory.start_time(), 3.0) self.assertEqual(trajectory.end_time(), 4.0) self.assertTrue(trajectory.has_derivative()) + self.assertEqual(repr(trajectory), "CustomTrajectory()") numpy_compare.assert_float_equal(trajectory.value(t=1.5), np.array([[2.5, 3.5]])) numpy_compare.assert_float_equal( @@ -137,12 +141,18 @@ def test_custom_trajectory(self): numpy_compare.assert_float_equal( trajectory.EvalDerivative(t=2.3, derivative_order=2), np.zeros((1, 2))) + clone = trajectory.Clone() numpy_compare.assert_float_equal(clone.value(t=1.5), np.array([[2.5, 3.5]])) + self.assertEqual(repr(clone), "_WrappedTrajectory(CustomTrajectory())") + deriv = trajectory.MakeDerivative(derivative_order=1) numpy_compare.assert_float_equal( deriv.value(t=2.3), np.ones((1, 2))) + self.assertIn( + "_WrappedTrajectory(> WrapPyTrajectory(py::object py_traj) { + DRAKE_THROW_UNLESS(!py_traj.is_none()); + // Convert py_traj to a shared_ptr> whose C++ lifetime keeps + // the python object alive. + Trajectory* cpp_traj = py::cast*>(py_traj); + DRAKE_THROW_UNLESS(cpp_traj != nullptr); + std::shared_ptr> shared_cpp_traj( + /* stored pointer = */ cpp_traj, + /* deleter = */ [captured_py_traj = std::move(py_traj)]( // BR + void*) mutable { + py::gil_scoped_acquire deleter_guard; + captured_py_traj = py::none(); + }); + // Wrap it in a unique_ptr to meet out required return signature. + return std::make_unique>( + std::move(shared_cpp_traj)); + } + // Trampoline virtual methods. 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); + py::gil_scoped_acquire guard; + // Trajectory subclasses in Python must implement cloning by defining + // either a __deepcopy__ (preferred) or Clone (legacy) method. We'll try + // Clone first so it has priority, but if it doesn't exist we'll fall back + // to __deepcopy__ and just let the "no such method deepcopy" error + // message propagate if both were missing. Because the + // PYBIND11_OVERLOAD_INT macro embeds a `return ...;` statement, we must + // wrap it in lambda so that we can post-process the return value. + bool used_legacy_clone = true; + auto make_python_deepcopy = [&]() -> py::object { + PYBIND11_OVERLOAD_INT(py::object, Trajectory, "Clone"); + used_legacy_clone = false; + auto deepcopy = py::module_::import("copy").attr("deepcopy"); + return deepcopy(this); + }; + py::object copied = make_python_deepcopy(); + if (used_legacy_clone) { + WarnDeprecated( + fmt::format( + "Support for overriding {}.Clone as a virtual function is " + "deprecated. Subclasses should implement __deepcopy__, " + "instead.", + NiceTypeName::Get(*this)), + "2025-08-01"); + } + return WrapPyTrajectory(std::move(copied)); } MatrixX do_value(const T& t) const final { @@ -216,10 +261,18 @@ struct Impl { std::unique_ptr> DoMakeDerivative( 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); + py::gil_scoped_acquire guard; + // Because the PYBIND11_OVERLOAD_INT macro embeds a `return ...;` + // statement, we must wrap it in lambda so that we can post-process the + // return value. + auto make_python_derivative = [&]() -> py::object { + PYBIND11_OVERLOAD_INT( + py::object, Trajectory, "DoMakeDerivative", derivative_order); + // If the macro did not return, use the base class error message. + Base::DoMakeDerivative(derivative_order); + DRAKE_UNREACHABLE(); + }; + return WrapPyTrajectory(make_python_derivative()); } T do_start_time() const final { @@ -760,6 +813,23 @@ struct Impl { cls_doc.Append.doc); DefCopyAndDeepCopy(&cls); } + + { + using Class = trajectories::internal::WrappedTrajectory; + auto cls = DefineTemplateClassWithDefault>( + m, "_WrappedTrajectory", param, "(Internal use only)"); + cls // BR + .def(py::init([](const Trajectory& trajectory) { + // The keep_alive is responsible for object lifetime, so we'll give + // the constructor an unowned pointer. + return std::make_unique( + make_unowned_shared_ptr_from_raw(&trajectory)); + }), + py::arg("trajectory"), + // Keep alive, ownership: `return` keeps `trajectory` alive. + py::keep_alive<0, 1>()) + .def("unwrap", &Class::unwrap, py_rvp::reference_internal); + } } }; } // namespace diff --git a/common/trajectories/BUILD.bazel b/common/trajectories/BUILD.bazel index 1d35a2c5056c..f91ab5d9ba2b 100644 --- a/common/trajectories/BUILD.bazel +++ b/common/trajectories/BUILD.bazel @@ -26,6 +26,7 @@ drake_cc_package_library( ":piecewise_trajectory", ":stacked_trajectory", ":trajectory", + ":wrapped_trajectory", ], ) @@ -219,6 +220,15 @@ drake_cc_library( ], ) +drake_cc_library( + name = "wrapped_trajectory", + srcs = ["wrapped_trajectory.cc"], + hdrs = ["wrapped_trajectory.h"], + deps = [ + ":trajectory", + ], +) + # === test/ === drake_cc_googletest( @@ -388,4 +398,13 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "wrapped_trajectory_test", + deps = [ + ":function_handle_trajectory", + ":wrapped_trajectory", + "//common/test_utilities:expect_throws_message", + ], +) + add_lint_tests() diff --git a/common/trajectories/test/wrapped_trajectory_test.cc b/common/trajectories/test/wrapped_trajectory_test.cc new file mode 100644 index 000000000000..33133313500b --- /dev/null +++ b/common/trajectories/test/wrapped_trajectory_test.cc @@ -0,0 +1,57 @@ +#include "drake/common/trajectories/wrapped_trajectory.h" + +#include + +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/common/trajectories/function_handle_trajectory.h" + +namespace drake { +namespace trajectories { +namespace internal { +namespace { + +Eigen::Vector2d Circle(const double& t) { + return Eigen::Vector2d(std::sin(t), std::cos(t)); +} + +Eigen::Vector2d CircleDerivative(const double& t, int order) { + DRAKE_DEMAND(order == 2); + return -Circle(t); +} + +std::shared_ptr> MakeFunctionHandleTrajectory() { + const double start_time = 0; + const double end_time = 1; + auto result = std::make_shared>( + &Circle, 2, 1, start_time, end_time); + result->set_derivative(&CircleDerivative); + return result; +} + +GTEST_TEST(WrappedTrajectoryTest, BasicTest) { + const WrappedTrajectory dut(MakeFunctionHandleTrajectory()); + EXPECT_EQ(dut.rows(), 2); + EXPECT_EQ(dut.cols(), 1); + EXPECT_EQ(dut.start_time(), 0); + EXPECT_EQ(dut.end_time(), 1); + EXPECT_TRUE(dut.has_derivative()); + const double t = 0.25; + EXPECT_EQ(dut.value(t), Circle(t)); + EXPECT_EQ(dut.EvalDerivative(t, 2), -Circle(t)); + EXPECT_EQ(dut.MakeDerivative(2)->value(t), -Circle(t)); + + auto clone = dut.Clone(); + EXPECT_EQ(clone->rows(), 2); + EXPECT_EQ(clone->cols(), 1); + EXPECT_EQ(clone->start_time(), 0); + EXPECT_EQ(clone->end_time(), 1); + EXPECT_TRUE(clone->has_derivative()); + EXPECT_EQ(clone->value(t), Circle(t)); + EXPECT_EQ(clone->EvalDerivative(t, 2), -Circle(t)); + EXPECT_EQ(clone->MakeDerivative(2)->value(t), -Circle(t)); +} + +} // namespace +} // namespace internal +} // namespace trajectories +} // namespace drake diff --git a/common/trajectories/wrapped_trajectory.cc b/common/trajectories/wrapped_trajectory.cc new file mode 100644 index 000000000000..21c9cbcc60ff --- /dev/null +++ b/common/trajectories/wrapped_trajectory.cc @@ -0,0 +1,74 @@ +#include "drake/common/trajectories/wrapped_trajectory.h" + +namespace drake { +namespace trajectories { +namespace internal { + +template +WrappedTrajectory::WrappedTrajectory( + std::shared_ptr> trajectory) + : trajectory_(std::move(trajectory)) { + DRAKE_THROW_UNLESS(trajectory_ != nullptr); +} + +template +WrappedTrajectory::~WrappedTrajectory() = default; + +template +const Trajectory* WrappedTrajectory::unwrap() const { + return trajectory_.get(); +} + +template +std::unique_ptr> WrappedTrajectory::DoClone() const { + return trajectory_->Clone(); +} + +template +MatrixX WrappedTrajectory::do_value(const T& t) const { + return trajectory_->value(t); +} + +template +bool WrappedTrajectory::do_has_derivative() const { + return trajectory_->has_derivative(); +} + +template +MatrixX WrappedTrajectory::DoEvalDerivative(const T& t, + int derivative_order) const { + return trajectory_->EvalDerivative(t, derivative_order); +} + +template +std::unique_ptr> WrappedTrajectory::DoMakeDerivative( + int derivative_order) const { + return trajectory_->MakeDerivative(derivative_order); +} + +template +Eigen::Index WrappedTrajectory::do_rows() const { + return trajectory_->rows(); +} + +template +Eigen::Index WrappedTrajectory::do_cols() const { + return trajectory_->cols(); +} + +template +T WrappedTrajectory::do_start_time() const { + return trajectory_->start_time(); +} + +template +T WrappedTrajectory::do_end_time() const { + return trajectory_->end_time(); +} + +} // namespace internal +} // namespace trajectories +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::internal::WrappedTrajectory); diff --git a/common/trajectories/wrapped_trajectory.h b/common/trajectories/wrapped_trajectory.h new file mode 100644 index 000000000000..f4208951f853 --- /dev/null +++ b/common/trajectories/wrapped_trajectory.h @@ -0,0 +1,49 @@ +#pragma once + +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/trajectories/trajectory.h" + +namespace drake { +namespace trajectories { +namespace internal { + +/* A WrappedTrajectory delegates all calls to a nested Trajectory object +maintained as a shared_ptr. +@tparam_default_scalar */ +template +class WrappedTrajectory final : public Trajectory { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(WrappedTrajectory); + + /* Wraps the given trajectory, which must not be nullptr. */ + explicit WrappedTrajectory(std::shared_ptr> trajectory); + + ~WrappedTrajectory() final; + + /* Returns the underlying Trajectory. */ + const Trajectory* unwrap() const; + + 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; + T do_start_time() const final; + T do_end_time() const final; + + std::shared_ptr> trajectory_; +}; + +} // namespace internal +} // namespace trajectories +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::trajectories::internal::WrappedTrajectory);