Skip to content

Commit

Permalink
[multibody] Clarify Frame dependencies on Parameters vs Context (Robo…
Browse files Browse the repository at this point in the history 
…tLocomotion#21661)

Switch Context-bearing virtual methods from public-virtual to NVI,
with the NVI hooks only given Parameters (vs the whole Context).

Breaking change: multibody::Frame disallows downstream subclassing.
  • Loading branch information
@jwnimmer-tri
jwnimmer-tri authored Jul 30, 2024
1 parent f925d27 commit f82fc2d
Show file tree
Hide file tree
Showing 6 changed files with 151 additions and 68 deletions.
12 changes: 9 additions & 3 deletions bindings/pydrake/multibody/tree_py.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<RigidTransform<T>,
const systems::Context<T>&, const RigidTransform<T>&>(
&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<math::RotationMatrix<T>,
const systems::Context<T>&, const math::RotationMatrix<T>&>(
&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",
Expand Down
44 changes: 44 additions & 0 deletions multibody/tree/fixed_offset_frame.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,25 @@ FixedOffsetFrame<T>::FixedOffsetFrame(const std::string& name,
template <typename T>
FixedOffsetFrame<T>::~FixedOffsetFrame() = default;


template <typename T>
void FixedOffsetFrame<T>::SetPoseInParentFrame(
systems::Context<T>* context, const math::RigidTransform<T>& X_PF) const {
systems::BasicVector<T>& X_PF_parameter =
context->get_mutable_numeric_parameter(X_PF_parameter_index_);
X_PF_parameter.set_value(
Eigen::Map<const VectorX<T>>(X_PF.GetAsMatrix34().data(), 12, 1));
}

template <typename T>
math::RigidTransform<T> FixedOffsetFrame<T>::GetPoseInParentFrame(
const systems::Context<T>& context) const {
const systems::BasicVector<T>& X_PF_parameter =
context.get_numeric_parameter(X_PF_parameter_index_);
return math::RigidTransform<T>(Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
X_PF_parameter.get_value().data()));
}

template <typename T>
template <typename ToScalar>
std::unique_ptr<Frame<ToScalar>> FixedOffsetFrame<T>::TemplatedDoCloneToScalar(
Expand Down Expand Up @@ -57,6 +76,31 @@ FixedOffsetFrame<T>::DoCloneToScalar(
return TemplatedDoCloneToScalar(tree_clone);
}

template <typename T>
math::RigidTransform<T> FixedOffsetFrame<T>::DoCalcPoseInBodyFrame(
const systems::Parameters<T>& parameters) const {
// X_BF = X_BP * X_PF
const systems::BasicVector<T>& X_PF_parameter =
parameters.get_numeric_parameter(X_PF_parameter_index_);
const math::RigidTransform<T> X_PF =
math::RigidTransform<T>(Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
X_PF_parameter.get_value().data()));
return parent_frame_.CalcOffsetPoseInBody(parameters, X_PF);
}

template <typename T>
math::RotationMatrix<T> FixedOffsetFrame<T>::DoCalcRotationMatrixInBodyFrame(
const systems::Parameters<T>& parameters) const {
// R_BF = R_BP * R_PF
const systems::BasicVector<T>& X_PF_parameter =
parameters.get_numeric_parameter(X_PF_parameter_index_);
return parent_frame_.CalcOffsetRotationMatrixInBody(
parameters,
math::RotationMatrix<T>(Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
X_PF_parameter.get_value().data())
.template block<3, 3>(0, 0)));
}

} // namespace multibody
} // namespace drake

Expand Down
45 changes: 10 additions & 35 deletions multibody/tree/fixed_offset_frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,49 +69,18 @@ class FixedOffsetFrame final : public Frame<T> {

~FixedOffsetFrame() override;

math::RigidTransform<T> CalcPoseInBodyFrame(
const systems::Context<T>& context) const override {
// X_BF = X_BP * X_PF
const math::RigidTransform<T> X_PF = GetPoseInParentFrame(context);
return parent_frame_.CalcOffsetPoseInBody(context, X_PF);
}

math::RotationMatrix<T> CalcRotationMatrixInBodyFrame(
const systems::Context<T>& context) const override {
// R_BF = R_BP * R_PF
const systems::BasicVector<T>& X_PF_parameter =
context.get_numeric_parameter(X_PF_parameter_index_);
return parent_frame_.CalcOffsetRotationMatrixInBody(
context,
math::RotationMatrix<T>(Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
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<T>* context,
const math::RigidTransform<T>& X_PF) const {
systems::BasicVector<T>& X_PF_parameter =
context->get_mutable_numeric_parameter(X_PF_parameter_index_);
X_PF_parameter.set_value(
Eigen::Map<const VectorX<T>>(X_PF.GetAsMatrix34().data(), 12, 1));
}
const math::RigidTransform<T>& 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<T> GetPoseInParentFrame(
const systems::Context<T>& context) const {
const systems::BasicVector<T>& X_PF_parameter =
context.get_numeric_parameter(X_PF_parameter_index_);
return math::RigidTransform<T>(Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
X_PF_parameter.get_value().data()));
}
const systems::Context<T>& context) const;

/// @returns The default fixed pose in the body frame.
math::RigidTransform<T> GetFixedPoseInBodyFrame() const override {
Expand Down Expand Up @@ -148,6 +117,12 @@ class FixedOffsetFrame final : public Frame<T> {
const internal::MultibodyTree<symbolic::Expression>&) const override;
/// @}

math::RigidTransform<T> DoCalcPoseInBodyFrame(
const systems::Parameters<T>& parameters) const override;

math::RotationMatrix<T> DoCalcRotationMatrixInBodyFrame(
const systems::Parameters<T>& parameters) const override;

private:
// Implementation for Frame::DoDeclareFrameParameters().
// FixedOffsetFrame declares a single parameter for its RigidTransform.
Expand Down
69 changes: 61 additions & 8 deletions multibody/tree/frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,14 +84,22 @@ class Frame : public FrameBase<T> {
/// with this frame.
/// In particular, if `this` **is** the body frame B, this method directly
/// returns the identity transformation.
virtual math::RigidTransform<T> CalcPoseInBodyFrame(
const systems::Context<T>& 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<T> CalcPoseInBodyFrame(
const systems::Context<T>& 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<T> CalcRotationMatrixInBodyFrame(
const systems::Context<T>& 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<T> CalcRotationMatrixInBodyFrame(
const systems::Context<T>& 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.
Expand Down Expand Up @@ -124,6 +132,11 @@ class Frame : public FrameBase<T> {
"', 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.
Expand All @@ -134,22 +147,40 @@ class Frame : public FrameBase<T> {
/// identity transformation, this method directly returns `X_FQ`.
/// Specific frame subclasses can override this method to provide faster
/// implementations if needed.
virtual math::RigidTransform<T> CalcOffsetPoseInBody(
math::RigidTransform<T> CalcOffsetPoseInBody(
const systems::Context<T>& context,
const math::RigidTransform<T>& 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<T> CalcOffsetPoseInBody(
const systems::Parameters<T>& parameters,
const math::RigidTransform<T>& 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<T> CalcOffsetRotationMatrixInBody(
math::RotationMatrix<T> CalcOffsetRotationMatrixInBody(
const systems::Context<T>& context,
const math::RotationMatrix<T>& 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<T> CalcOffsetRotationMatrixInBody(
const systems::Parameters<T>& parameters,
const math::RotationMatrix<T>& 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.
Expand Down Expand Up @@ -507,6 +538,28 @@ class Frame : public FrameBase<T> {
const internal::MultibodyTree<symbolic::Expression>&) const = 0;
/// @}

// NVI for CalcPoseInBodyFrame.
virtual math::RigidTransform<T> DoCalcPoseInBodyFrame(
const systems::Parameters<T>& parameters) const = 0;

// NVI for CalcRotationMatrixInBodyFrame.
virtual math::RotationMatrix<T> DoCalcRotationMatrixInBodyFrame(
const systems::Parameters<T>& parameters) const = 0;

// NVI for CalcOffsetPoseInBody.
virtual math::RigidTransform<T> DoCalcOffsetPoseInBody(
const systems::Parameters<T>& parameters,
const math::RigidTransform<T>& X_FQ) const {
return DoCalcPoseInBodyFrame(parameters) * X_FQ;
}

// NVI for CalcOffsetRotationMatrixInBody.
virtual math::RotationMatrix<T> DoCalcOffsetRotationMatrixInBody(
const systems::Parameters<T>& parameters,
const math::RotationMatrix<T>& R_FQ) const {
return DoCalcRotationMatrixInBodyFrame(parameters) * R_FQ;
}

private:
// Implementation for MultibodyElement::DoSetTopology().
void DoSetTopology(const internal::MultibodyTreeTopology& tree_topology)
Expand Down
5 changes: 5 additions & 0 deletions multibody/tree/frame_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <typename T>
class FrameBase : public MultibodyElement<T> {
Expand Down
44 changes: 22 additions & 22 deletions multibody/tree/rigid_body.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,28 +61,6 @@ class RigidBodyFrame final : public Frame<T> {

~RigidBodyFrame() override;

math::RigidTransform<T> CalcPoseInBodyFrame(
const systems::Context<T>&) const override {
return math::RigidTransform<T>::Identity();
}

math::RotationMatrix<T> CalcRotationMatrixInBodyFrame(
const systems::Context<T>&) const override {
return math::RotationMatrix<T>::Identity();
}

math::RigidTransform<T> CalcOffsetPoseInBody(
const systems::Context<T>&,
const math::RigidTransform<T>& X_FQ) const override {
return X_FQ;
}

math::RotationMatrix<T> CalcOffsetRotationMatrixInBody(
const systems::Context<T>&,
const math::RotationMatrix<T>& R_FQ) const override {
return R_FQ;
}

math::RigidTransform<T> GetFixedPoseInBodyFrame() const override {
return math::RigidTransform<T>::Identity();
}
Expand Down Expand Up @@ -112,6 +90,28 @@ class RigidBodyFrame final : public Frame<T> {
std::unique_ptr<Frame<symbolic::Expression>> DoCloneToScalar(
const internal::MultibodyTree<symbolic::Expression>&) const override;

math::RigidTransform<T> DoCalcPoseInBodyFrame(
const systems::Parameters<T>&) const override {
return math::RigidTransform<T>::Identity();
}

math::RotationMatrix<T> DoCalcRotationMatrixInBodyFrame(
const systems::Parameters<T>&) const override {
return math::RotationMatrix<T>::Identity();
}

math::RigidTransform<T> DoCalcOffsetPoseInBody(
const systems::Parameters<T>&,
const math::RigidTransform<T>& X_FQ) const override {
return X_FQ;
}

math::RotationMatrix<T> DoCalcOffsetRotationMatrixInBody(
const systems::Parameters<T>&,
const math::RotationMatrix<T>& R_FQ) const override {
return R_FQ;
}

private:
// RigidBody<T> and RigidBodyFrame<T> are natural allies. A RigidBodyFrame
// object is created every time a RigidBody object is created and they are
Expand Down

0 comments on commit f82fc2d

Please sign in to comment.