Skip to content

Commit 2ca65e8

Browse files
sherm1sherm1
committed
Nuke useless FrameBase class; adds frame body pose cache.
Convert BodyNode functions to use the new cache.
1 parent 1ec9904 commit 2ca65e8

14 files changed

+424
-189
lines changed

‎multibody/tree/BUILD.bazel

+2-2
Original file line numberDiff line numberDiff line change
@@ -59,13 +59,15 @@ drake_cc_library(
5959
"acceleration_kinematics_cache.cc",
6060
"articulated_body_force_cache.cc",
6161
"articulated_body_inertia_cache.cc",
62+
"frame_body_pose_cache.cc",
6263
"position_kinematics_cache.cc",
6364
"velocity_kinematics_cache.cc",
6465
],
6566
hdrs = [
6667
"acceleration_kinematics_cache.h",
6768
"articulated_body_force_cache.h",
6869
"articulated_body_inertia_cache.h",
70+
"frame_body_pose_cache.h",
6971
"position_kinematics_cache.h",
7072
"velocity_kinematics_cache.h",
7173
],
@@ -89,7 +91,6 @@ drake_cc_library(
8991
"fixed_offset_frame.cc",
9092
"force_element.cc",
9193
"frame.cc",
92-
"frame_base.cc",
9394
"joint.cc",
9495
"joint_actuator.cc",
9596
"linear_bushing_roll_pitch_yaw.cc",
@@ -133,7 +134,6 @@ drake_cc_library(
133134
"fixed_offset_frame.h",
134135
"force_element.h",
135136
"frame.h",
136-
"frame_base.h",
137137
"joint.h",
138138
"joint_actuator.h",
139139
"linear_bushing_roll_pitch_yaw.h",

‎multibody/tree/body_node.h

+56-31
Original file line numberDiff line numberDiff line change
@@ -185,8 +185,11 @@ class BodyNode : public MultibodyElement<T> {
185185
// @pre CalcPositionKinematicsCache_BaseToTip() must have already been called
186186
// for the parent node (and, by recursive precondition, all predecessor nodes
187187
// in the tree.)
188+
189+
// TODO(sherm1) This function should not take a context.
188190
void CalcPositionKinematicsCache_BaseToTip(
189191
const systems::Context<T>& context,
192+
const FrameBodyPoseCache<T>& frame_body_pose_cache,
190193
PositionKinematicsCache<T>* pc) const {
191194
// This method must not be called for the "world" body node.
192195
DRAKE_ASSERT(topology_.rigid_body != world_index());
@@ -203,7 +206,7 @@ class BodyNode : public MultibodyElement<T> {
203206
// mobilizer. q(W:P) denotes all generalized positions in the kinematics
204207
// path between World and the parent body P. It assumes we are in a
205208
// base-to-tip recursion and therefore `X_WP` has already been updated.
206-
CalcAcrossMobilizerBodyPoses_BaseToTip(context, pc);
209+
CalcAcrossMobilizerBodyPoses_BaseToTip(frame_body_pose_cache, pc);
207210

208211
// TODO(amcastro-tri):
209212
// Update Body specific kinematics. These include:
@@ -241,6 +244,8 @@ class BodyNode : public MultibodyElement<T> {
241244
// Unit test coverage for this method is provided, among others, in
242245
// double_pendulum_test.cc, and by any other unit tests making use of
243246
// MultibodyTree::CalcVelocityKinematicsCache().
247+
248+
// TODO(sherm1) This function should not take a context.
244249
void CalcVelocityKinematicsCache_BaseToTip(
245250
const systems::Context<T>& context,
246251
const PositionKinematicsCache<T>& pc,
@@ -379,8 +384,11 @@ class BodyNode : public MultibodyElement<T> {
379384
// Unit test coverage for this method is provided, among others, in
380385
// double_pendulum_test.cc, and by any other unit tests making use of
381386
// MultibodyTree::CalcAccelerationKinematicsCache().
387+
388+
// TODO(sherm1) This function should not take a context.
382389
void CalcSpatialAcceleration_BaseToTip(
383390
const systems::Context<T>& context,
391+
const FrameBodyPoseCache<T>& frame_body_poses_cache,
384392
const PositionKinematicsCache<T>& pc,
385393
const VelocityKinematicsCache<T>* vc,
386394
const VectorX<T>& mbt_vdot,
@@ -456,12 +464,11 @@ class BodyNode : public MultibodyElement<T> {
456464
// =========================================================================
457465
// Computation of A_PB = DtP(V_PB), Eq. (4).
458466

459-
// TODO(amcastro-tri): consider caching these. Also used in velocity
460-
// kinematics.
461-
const math::RotationMatrix<T> R_PF =
462-
frame_F.CalcRotationMatrixInBodyFrame(context);
463-
const math::RigidTransform<T> X_MB =
464-
frame_M.CalcPoseInBodyFrame(context).inverse();
467+
const math::RigidTransform<T>& X_PF =
468+
frame_F.get_X_BF(frame_body_poses_cache); // B==P
469+
const math::RotationMatrix<T>& R_PF = X_PF.rotation();
470+
const math::RigidTransform<T>& X_MB =
471+
frame_M.get_X_FB(frame_body_poses_cache); // F==M
465472

466473
// Form the rotation matrix relating the world frame W and parent body P.
467474
// Available since we are called within a base-to-tip recursion.
@@ -593,8 +600,11 @@ class BodyNode : public MultibodyElement<T> {
593600
// Unit test coverage for this method is provided, among others, in
594601
// double_pendulum_test.cc, and by any other unit tests making use of
595602
// MultibodyTree::CalcInverseDynamics().
603+
604+
// TODO(sherm1) This function should not take a context.
596605
void CalcInverseDynamics_TipToBase(
597606
const systems::Context<T>& context,
607+
const FrameBodyPoseCache<T>& frame_body_pose_cache,
598608
const PositionKinematicsCache<T>& pc,
599609
const std::vector<SpatialInertia<T>>& M_B_W_cache,
600610
const std::vector<SpatialForce<T>>* Fb_Bo_W_cache,
@@ -675,7 +685,8 @@ class BodyNode : public MultibodyElement<T> {
675685
// Compute shift vector from Bo to Mo expressed in the world frame W.
676686
const Frame<T>& frame_M = outboard_frame();
677687
DRAKE_DEMAND(frame_M.body().index() == body_B.index());
678-
const math::RigidTransform<T> X_BM = frame_M.CalcPoseInBodyFrame(context);
688+
const math::RigidTransform<T>& X_BM =
689+
frame_M.get_X_BF(frame_body_pose_cache); // F==M
679690
const Vector3<T>& p_BoMo_B = X_BM.translation();
680691
const math::RigidTransform<T>& X_WB = get_X_WB(pc);
681692
const math::RotationMatrix<T>& R_WB = X_WB.rotation();
@@ -700,8 +711,8 @@ class BodyNode : public MultibodyElement<T> {
700711
// p_CoMc_W:
701712
const Frame<T>& frame_Mc = child_node->outboard_frame();
702713
const math::RotationMatrix<T>& R_WC = child_node->get_X_WB(pc).rotation();
703-
const math::RigidTransform<T> X_CMc =
704-
frame_Mc.CalcPoseInBodyFrame(context);
714+
const math::RigidTransform<T>& X_CMc =
715+
frame_Mc.get_X_BF(frame_body_pose_cache); // B==C, F==Mc
705716
const Vector3<T>& p_CoMc_W = R_WC * X_CMc.translation();
706717

707718
// Shift position vector from child C outboard mobilizer frame Mc to body
@@ -773,8 +784,11 @@ class BodyNode : public MultibodyElement<T> {
773784
//
774785
// @pre The position kinematics cache `pc` was already updated to be in sync
775786
// with `context` by MultibodyTree::CalcPositionKinematicsCache().
787+
788+
// TODO(sherm1) This function should not take a context.
776789
void CalcAcrossNodeJacobianWrtVExpressedInWorld(
777790
const systems::Context<T>& context,
791+
const FrameBodyPoseCache<T>& frame_body_pose_cache,
778792
const PositionKinematicsCache<T>& pc,
779793
EigenPtr<MatrixX<T>> H_PB_W) const {
780794
// Checks on the input arguments.
@@ -788,10 +802,11 @@ class BodyNode : public MultibodyElement<T> {
788802
// Outboard frame M of this node's mobilizer.
789803
const Frame<T>& frame_M = outboard_frame();
790804

791-
const math::RotationMatrix<T> R_PF =
792-
frame_F.CalcRotationMatrixInBodyFrame(context);
793-
const math::RigidTransform<T> X_MB =
794-
frame_M.CalcPoseInBodyFrame(context).inverse();
805+
const math::RigidTransform<T>& X_PF =
806+
frame_F.get_X_BF(frame_body_pose_cache); // B==P
807+
const math::RotationMatrix<T>& R_PF = X_PF.rotation();
808+
const math::RigidTransform<T>& X_MB =
809+
frame_M.get_X_FB(frame_body_pose_cache); // F==M
795810

796811
// Form the rotation matrix relating the world frame W and parent body P.
797812
const math::RotationMatrix<T>& R_WP = get_R_WP(pc);
@@ -900,6 +915,8 @@ class BodyNode : public MultibodyElement<T> {
900915
// - Revolute: [x y z 0 0 0]
901916
// - Prismatic: [0 0 0 x y z]
902917
// - Ball: 3x3 blocks of zeroes.
918+
919+
// TODO(sherm1) This function should not take a context.
903920
void CalcArticulatedBodyInertiaCache_TipToBase(
904921
const systems::Context<T>& context,
905922
const PositionKinematicsCache<T>& pc,
@@ -1094,6 +1111,8 @@ class BodyNode : public MultibodyElement<T> {
10941111
//
10951112
// @throws when called on the _root_ node or `aba_force_cache` is
10961113
// nullptr.
1114+
1115+
// TODO(sherm1) This function should not take a context.
10971116
void CalcArticulatedBodyForceCache_TipToBase(
10981117
const systems::Context<T>& context,
10991118
const PositionKinematicsCache<T>& pc,
@@ -1186,6 +1205,8 @@ class BodyNode : public MultibodyElement<T> {
11861205
// called for the parent node (and, by recursive precondition, all
11871206
// predecessor nodes in the tree.)
11881207
// @throws when called on the _root_ node of `ac` or `vdot` is nullptr.
1208+
1209+
// TODO(sherm1) This function should not take a context.
11891210
void CalcArticulatedBodyAccelerations_BaseToTip(
11901211
const systems::Context<T>& context,
11911212
const PositionKinematicsCache<T>& pc,
@@ -1296,10 +1317,14 @@ class BodyNode : public MultibodyElement<T> {
12961317
// @pre pc and vc previously computed to be in sync with `context.
12971318
//
12981319
// @throws when `Ab_WB` is nullptr.
1299-
void CalcSpatialAccelerationBias(const systems::Context<T>& context,
1300-
const PositionKinematicsCache<T>& pc,
1301-
const VelocityKinematicsCache<T>& vc,
1302-
SpatialAcceleration<T>* Ab_WB) const {
1320+
1321+
// TODO(sherm1) This function should not take a context.
1322+
void CalcSpatialAccelerationBias(
1323+
const systems::Context<T>& context,
1324+
const FrameBodyPoseCache<T>& frame_body_pose_cache,
1325+
const PositionKinematicsCache<T>& pc,
1326+
const VelocityKinematicsCache<T>& vc,
1327+
SpatialAcceleration<T>* Ab_WB) const {
13031328
DRAKE_THROW_UNLESS(Ab_WB != nullptr);
13041329
// As a guideline for developers, please refer to @ref
13051330
// abi_computing_accelerations for a detailed description and derivation of
@@ -1311,10 +1336,11 @@ class BodyNode : public MultibodyElement<T> {
13111336
const Frame<T>& frame_M = outboard_frame();
13121337

13131338
// Compute R_PF and X_MB.
1314-
const math::RotationMatrix<T> R_PF =
1315-
frame_F.CalcRotationMatrixInBodyFrame(context);
1316-
const math::RigidTransform<T> X_MB =
1317-
frame_M.CalcPoseInBodyFrame(context).inverse();
1339+
const math::RigidTransform<T>& X_PF =
1340+
frame_F.get_X_BF(frame_body_pose_cache); // B==P
1341+
const math::RotationMatrix<T>& R_PF = X_PF.rotation();
1342+
const math::RigidTransform<T>& X_MB =
1343+
frame_M.get_X_FB(frame_body_pose_cache); // F==M
13181344

13191345
// Parent position in the world is available from the position kinematics.
13201346
const math::RotationMatrix<T>& R_WP = get_R_WP(pc);
@@ -1733,7 +1759,7 @@ class BodyNode : public MultibodyElement<T> {
17331759
// between the world and the parent body P. It assumes we are in a base-to-tip
17341760
// recursion and therefore `X_WP` has already been updated.
17351761
void CalcAcrossMobilizerBodyPoses_BaseToTip(
1736-
const systems::Context<T>& context,
1762+
const FrameBodyPoseCache<T>& frame_body_pose_cache,
17371763
PositionKinematicsCache<T>* pc) const {
17381764
// RigidBody for this node.
17391765
const RigidBody<T>& body_B = body();
@@ -1753,8 +1779,10 @@ class BodyNode : public MultibodyElement<T> {
17531779
// - X_FM(q_B)
17541780
// - X_WP(q(W:B)), where q(W:B) includes all positions in the kinematics
17551781
// path from body B to the world W.
1756-
const math::RigidTransform<T> X_MB =
1757-
frame_M.CalcPoseInBodyFrame(context).inverse();
1782+
const math::RigidTransform<T>& X_MB =
1783+
frame_M.get_X_FB(frame_body_pose_cache); // F==M
1784+
const math::RigidTransform<T>& X_PF =
1785+
frame_F.get_X_BF(frame_body_pose_cache); // B==P
17581786
const math::RigidTransform<T>& X_FM =
17591787
get_X_FM(*pc); // mobilizer.Eval_X_FM(ctx)
17601788
const math::RigidTransform<T>& X_WP =
@@ -1771,12 +1799,7 @@ class BodyNode : public MultibodyElement<T> {
17711799
// In that case X_FB = X_FM as suggested by setting X_MB = Identity.
17721800
const math::RigidTransform<T> X_FB = X_FM * X_MB;
17731801

1774-
// Given the pose X_FB of body frame B measured in the mobilizer inboard
1775-
// frame F, we can ask frame F (who's parent body is P) for the pose of body
1776-
// B measured in the frame of the parent body P.
1777-
// In the particular case F = B, this method directly returns X_FB.
1778-
X_PB = frame_F.CalcOffsetPoseInBody(context, X_FB);
1779-
1802+
X_PB = X_PF * X_FB;
17801803
X_WB = X_WP * X_PB;
17811804

17821805
// Compute shift vector p_PoBo_W from the parent origin to the body origin.
@@ -1803,6 +1826,8 @@ class BodyNode : public MultibodyElement<T> {
18031826
// quantities associated with `this` mobilizer. MultibodyTree will always
18041827
// provide a valid PositionKinematicsCache pointer, otherwise this method
18051828
// aborts in Debug builds.
1829+
1830+
// TODO(sherm1) This function should not take a context.
18061831
void CalcAcrossMobilizerPositionKinematicsCache(
18071832
const systems::Context<T>& context,
18081833
PositionKinematicsCache<T>* pc) const {

0 commit comments

Comments
 (0)