diff --git a/bindings/pydrake/multibody/tree_py.cc b/bindings/pydrake/multibody/tree_py.cc index 6b4a5b343bfb..61f7d646d39f 100644 --- a/bindings/pydrake/multibody/tree_py.cc +++ b/bindings/pydrake/multibody/tree_py.cc @@ -221,12 +221,18 @@ void DoScalarDependentDefinitions(py::module m, T) { .def("GetFixedRotationMatrixInBodyFrame", &Class::GetFixedRotationMatrixInBodyFrame, cls_doc.GetFixedRotationMatrixInBodyFrame.doc) - .def("CalcOffsetPoseInBody", &Class::CalcOffsetPoseInBody, + .def("CalcOffsetPoseInBody", + overload_cast_explicit, + const systems::Context&, const RigidTransform&>( + &Class::CalcOffsetPoseInBody), py::arg("context"), py::arg("X_FQ"), cls_doc.CalcOffsetPoseInBody.doc) .def("CalcOffsetRotationMatrixInBody", - &Class::CalcOffsetRotationMatrixInBody, py::arg("context"), - py::arg("R_FQ"), cls_doc.CalcOffsetRotationMatrixInBody.doc) + overload_cast_explicit, + const systems::Context&, const math::RotationMatrix&>( + &Class::CalcOffsetRotationMatrixInBody), + py::arg("context"), py::arg("R_FQ"), + cls_doc.CalcOffsetRotationMatrixInBody.doc) .def("GetFixedOffsetPoseInBody", &Class::GetFixedOffsetPoseInBody, py::arg("X_FQ"), cls_doc.GetFixedOffsetPoseInBody.doc) .def("GetFixedRotationMatrixInBody", diff --git a/multibody/tree/fixed_offset_frame.cc b/multibody/tree/fixed_offset_frame.cc index d2f5a822bd3d..f6260004c0f1 100644 --- a/multibody/tree/fixed_offset_frame.cc +++ b/multibody/tree/fixed_offset_frame.cc @@ -28,6 +28,25 @@ FixedOffsetFrame::FixedOffsetFrame(const std::string& name, template FixedOffsetFrame::~FixedOffsetFrame() = default; + +template +void FixedOffsetFrame::SetPoseInParentFrame( + systems::Context* context, const math::RigidTransform& X_PF) const { + systems::BasicVector& X_PF_parameter = + context->get_mutable_numeric_parameter(X_PF_parameter_index_); + X_PF_parameter.set_value( + Eigen::Map>(X_PF.GetAsMatrix34().data(), 12, 1)); +} + +template +math::RigidTransform FixedOffsetFrame::GetPoseInParentFrame( + const systems::Context& context) const { + const systems::BasicVector& X_PF_parameter = + context.get_numeric_parameter(X_PF_parameter_index_); + return math::RigidTransform(Eigen::Map>( + X_PF_parameter.get_value().data())); +} + template template std::unique_ptr> FixedOffsetFrame::TemplatedDoCloneToScalar( @@ -57,6 +76,31 @@ FixedOffsetFrame::DoCloneToScalar( return TemplatedDoCloneToScalar(tree_clone); } +template +math::RigidTransform FixedOffsetFrame::DoCalcPoseInBodyFrame( + const systems::Parameters& parameters) const { + // X_BF = X_BP * X_PF + const systems::BasicVector& X_PF_parameter = + parameters.get_numeric_parameter(X_PF_parameter_index_); + const math::RigidTransform X_PF = + math::RigidTransform(Eigen::Map>( + X_PF_parameter.get_value().data())); + return parent_frame_.CalcOffsetPoseInBody(parameters, X_PF); +} + +template +math::RotationMatrix FixedOffsetFrame::DoCalcRotationMatrixInBodyFrame( + const systems::Parameters& parameters) const { + // R_BF = R_BP * R_PF + const systems::BasicVector& X_PF_parameter = + parameters.get_numeric_parameter(X_PF_parameter_index_); + return parent_frame_.CalcOffsetRotationMatrixInBody( + parameters, + math::RotationMatrix(Eigen::Map>( + X_PF_parameter.get_value().data()) + .template block<3, 3>(0, 0))); +} + } // namespace multibody } // namespace drake diff --git a/multibody/tree/fixed_offset_frame.h b/multibody/tree/fixed_offset_frame.h index 8e9e89c4cb84..c5ee64b4fded 100644 --- a/multibody/tree/fixed_offset_frame.h +++ b/multibody/tree/fixed_offset_frame.h @@ -69,49 +69,18 @@ class FixedOffsetFrame final : public Frame { ~FixedOffsetFrame() override; - math::RigidTransform CalcPoseInBodyFrame( - const systems::Context& context) const override { - // X_BF = X_BP * X_PF - const math::RigidTransform X_PF = GetPoseInParentFrame(context); - return parent_frame_.CalcOffsetPoseInBody(context, X_PF); - } - - math::RotationMatrix CalcRotationMatrixInBodyFrame( - const systems::Context& context) const override { - // R_BF = R_BP * R_PF - const systems::BasicVector& X_PF_parameter = - context.get_numeric_parameter(X_PF_parameter_index_); - return parent_frame_.CalcOffsetRotationMatrixInBody( - context, - math::RotationMatrix(Eigen::Map>( - X_PF_parameter.get_value().data()) - .template block<3, 3>(0, 0))); - } - /// Sets the pose of `this` frame F in its parent frame P. - /// @param[in] context contains the state of the multibody plant. + /// @param[in,out] context of the multibody plant associated with this frame. /// @param[in] X_PF Rigid transform that characterizes `this` frame F's pose /// (orientation and position) in its parent frame P. - /// @pre `this` frame has been registered in the given `context`. void SetPoseInParentFrame(systems::Context* context, - const math::RigidTransform& X_PF) const { - systems::BasicVector& X_PF_parameter = - context->get_mutable_numeric_parameter(X_PF_parameter_index_); - X_PF_parameter.set_value( - Eigen::Map>(X_PF.GetAsMatrix34().data(), 12, 1)); - } + const math::RigidTransform& X_PF) const; /// Returns the rigid transform X_PF that characterizes `this` frame F's pose /// in its parent frame P. - /// @param[in] context contains the state of the multibody plant. - /// @pre `this` frame has been registered in the given `context`. + /// @param[in] context of the multibody plant associated with this frame. math::RigidTransform GetPoseInParentFrame( - const systems::Context& context) const { - const systems::BasicVector& X_PF_parameter = - context.get_numeric_parameter(X_PF_parameter_index_); - return math::RigidTransform(Eigen::Map>( - X_PF_parameter.get_value().data())); - } + const systems::Context& context) const; /// @returns The default fixed pose in the body frame. math::RigidTransform GetFixedPoseInBodyFrame() const override { @@ -148,6 +117,12 @@ class FixedOffsetFrame final : public Frame { const internal::MultibodyTree&) const override; /// @} + math::RigidTransform DoCalcPoseInBodyFrame( + const systems::Parameters& parameters) const override; + + math::RotationMatrix DoCalcRotationMatrixInBodyFrame( + const systems::Parameters& parameters) const override; + private: // Implementation for Frame::DoDeclareFrameParameters(). // FixedOffsetFrame declares a single parameter for its RigidTransform. diff --git a/multibody/tree/frame.h b/multibody/tree/frame.h index 2e9341d8d820..94842529042a 100644 --- a/multibody/tree/frame.h +++ b/multibody/tree/frame.h @@ -84,14 +84,22 @@ class Frame : public FrameBase { /// with this frame. /// In particular, if `this` **is** the body frame B, this method directly /// returns the identity transformation. - virtual math::RigidTransform CalcPoseInBodyFrame( - const systems::Context& context) const = 0; + /// Note that this ONLY depends on the Parameters in the context; it does + /// not depend on time, input, state, etc. + math::RigidTransform CalcPoseInBodyFrame( + const systems::Context& context) const { + return DoCalcPoseInBodyFrame(context.get_parameters()); + } /// Returns the rotation matrix `R_BF` that relates body frame B to `this` /// frame F (B is the body frame to which `this` frame F is attached). /// @note If `this` is B, this method returns the identity RotationMatrix. - virtual math::RotationMatrix CalcRotationMatrixInBodyFrame( - const systems::Context& context) const = 0; + /// Note that this ONLY depends on the Parameters in the context; it does + /// not depend on time, input, state, etc. + math::RotationMatrix CalcRotationMatrixInBodyFrame( + const systems::Context& context) const { + return DoCalcRotationMatrixInBodyFrame(context.get_parameters()); + } /// Variant of CalcPoseInBodyFrame() that returns the fixed pose `X_BF` of /// `this` frame F in the body frame B associated with this frame. @@ -124,6 +132,11 @@ class Frame : public FrameBase { "', which does not support this method."); } + // TODO(jwnimmer-tri) These next four functions only exist so that BodyFrame + // can override their NVI body to be a simple copy instead of ComposeXX or + // ComposeRR against an identity. It is not at all clear to me that this + // implementation complexity is buying us any measurable speedup at runtime. + /// Given the offset pose `X_FQ` of a frame Q in `this` frame F, this method /// computes the pose `X_BQ` of frame Q in the body frame B to which this /// frame is attached. @@ -134,22 +147,40 @@ class Frame : public FrameBase { /// identity transformation, this method directly returns `X_FQ`. /// Specific frame subclasses can override this method to provide faster /// implementations if needed. - virtual math::RigidTransform CalcOffsetPoseInBody( + math::RigidTransform CalcOffsetPoseInBody( const systems::Context& context, const math::RigidTransform& X_FQ) const { - return CalcPoseInBodyFrame(context) * X_FQ; + return DoCalcOffsetPoseInBody(context.get_parameters(), X_FQ); } +#ifndef DRAKE_DOXYGEN_CXX + // (Internal use only) Overload on Parameters instead of Context. + math::RigidTransform CalcOffsetPoseInBody( + const systems::Parameters& parameters, + const math::RigidTransform& X_FQ) const { + return DoCalcOffsetPoseInBody(parameters, X_FQ); + } +#endif + /// Calculates and returns the rotation matrix `R_BQ` that relates body frame /// B to frame Q via `this` intermediate frame F, i.e., `R_BQ = R_BF * R_FQ` /// (B is the body frame to which `this` frame F is attached). /// @param[in] R_FQ rotation matrix that relates frame F to frame Q. - virtual math::RotationMatrix CalcOffsetRotationMatrixInBody( + math::RotationMatrix CalcOffsetRotationMatrixInBody( const systems::Context& context, const math::RotationMatrix& R_FQ) const { - return CalcRotationMatrixInBodyFrame(context) * R_FQ; + return DoCalcOffsetRotationMatrixInBody(context.get_parameters(), R_FQ); } +#ifndef DRAKE_DOXYGEN_CXX + // (Internal use only) Overload on Parameters instead of Context. + math::RotationMatrix CalcOffsetRotationMatrixInBody( + const systems::Parameters& parameters, + const math::RotationMatrix& R_FQ) const { + return DoCalcOffsetRotationMatrixInBody(parameters, R_FQ); + } +#endif + /// Variant of CalcOffsetPoseInBody() that given the offset pose `X_FQ` of a /// frame Q in `this` frame F, returns the pose `X_BQ` of frame Q in the body /// frame B to which this frame is attached. @@ -507,6 +538,28 @@ class Frame : public FrameBase { const internal::MultibodyTree&) const = 0; /// @} + // NVI for CalcPoseInBodyFrame. + virtual math::RigidTransform DoCalcPoseInBodyFrame( + const systems::Parameters& parameters) const = 0; + + // NVI for CalcRotationMatrixInBodyFrame. + virtual math::RotationMatrix DoCalcRotationMatrixInBodyFrame( + const systems::Parameters& parameters) const = 0; + + // NVI for CalcOffsetPoseInBody. + virtual math::RigidTransform DoCalcOffsetPoseInBody( + const systems::Parameters& parameters, + const math::RigidTransform& X_FQ) const { + return DoCalcPoseInBodyFrame(parameters) * X_FQ; + } + + // NVI for CalcOffsetRotationMatrixInBody. + virtual math::RotationMatrix DoCalcOffsetRotationMatrixInBody( + const systems::Parameters& parameters, + const math::RotationMatrix& R_FQ) const { + return DoCalcRotationMatrixInBodyFrame(parameters) * R_FQ; + } + private: // Implementation for MultibodyElement::DoSetTopology(). void DoSetTopology(const internal::MultibodyTreeTopology& tree_topology) diff --git a/multibody/tree/frame_base.h b/multibody/tree/frame_base.h index e5ff020e94fe..32f14254334a 100644 --- a/multibody/tree/frame_base.h +++ b/multibody/tree/frame_base.h @@ -41,6 +41,11 @@ namespace multibody { /// of frames, most importantly whether a frame is associated with a material /// point of a body. /// +/// Even though the FrameBase class is virtual, it is not open for subclassing +/// by third-party code. All subclasses of Frame must exist as a closed set of +/// types inside of Drake. In other words, Drake's Stable API does NOT allow +/// subclassing any of FrameBase, Frame, BodyFrame, FixedOffsetFrame, etc. +/// /// @tparam_default_scalar template class FrameBase : public MultibodyElement { diff --git a/multibody/tree/rigid_body.h b/multibody/tree/rigid_body.h index 38a720a3c596..fa01a1d34a60 100644 --- a/multibody/tree/rigid_body.h +++ b/multibody/tree/rigid_body.h @@ -61,28 +61,6 @@ class RigidBodyFrame final : public Frame { ~RigidBodyFrame() override; - math::RigidTransform CalcPoseInBodyFrame( - const systems::Context&) const override { - return math::RigidTransform::Identity(); - } - - math::RotationMatrix CalcRotationMatrixInBodyFrame( - const systems::Context&) const override { - return math::RotationMatrix::Identity(); - } - - math::RigidTransform CalcOffsetPoseInBody( - const systems::Context&, - const math::RigidTransform& X_FQ) const override { - return X_FQ; - } - - math::RotationMatrix CalcOffsetRotationMatrixInBody( - const systems::Context&, - const math::RotationMatrix& R_FQ) const override { - return R_FQ; - } - math::RigidTransform GetFixedPoseInBodyFrame() const override { return math::RigidTransform::Identity(); } @@ -112,6 +90,28 @@ class RigidBodyFrame final : public Frame { std::unique_ptr> DoCloneToScalar( const internal::MultibodyTree&) const override; + math::RigidTransform DoCalcPoseInBodyFrame( + const systems::Parameters&) const override { + return math::RigidTransform::Identity(); + } + + math::RotationMatrix DoCalcRotationMatrixInBodyFrame( + const systems::Parameters&) const override { + return math::RotationMatrix::Identity(); + } + + math::RigidTransform DoCalcOffsetPoseInBody( + const systems::Parameters&, + const math::RigidTransform& X_FQ) const override { + return X_FQ; + } + + math::RotationMatrix DoCalcOffsetRotationMatrixInBody( + const systems::Parameters&, + const math::RotationMatrix& R_FQ) const override { + return R_FQ; + } + private: // RigidBody and RigidBodyFrame are natural allies. A RigidBodyFrame // object is created every time a RigidBody object is created and they are